hogman3d.cpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. K├╝mmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <iostream>
00018 #include <string>
00019 #include <algorithm>
00020 #include <iomanip>
00021 #include <sys/time.h>
00022 
00023 #include "hogman_minimal/graph_optimizer_hogman/graph_optimizer3d_chol.h"
00024 #include "hogman_minimal/graph_optimizer_hogman/graph_optimizer3d_hchol.h"
00025 #include "hogman_minimal/graph/loadEdges3d.h"
00026 #include "hogman_minimal/stuff/filesys_tools.h"
00027 #include "hogman_minimal/stuff/string_tools.h"
00028 #include "hogman_minimal/stuff/macros.h"
00029 #include "hogman_minimal/stuff/color_macros.h"
00030 
00031 using namespace std;
00032 using namespace AISNavigation;
00033 
00034 static const char * banner[]={
00035   "*******************************************************************",
00036   "*                           HOG-Man v 0.1                         *",
00037   "*              (c) Giorgio Grisetti, Rainer Kuemmerle,            *",
00038   "*                  Cyrill Stachniss                               *",
00039   "*******************************************************************",
00040   "",
00041   "usage: hogman3d [options] <graph_file>",
00042   "",
00043   "options:",
00044   " -hogman | -chol            selects the optimization strategy",
00045   "                            between",
00046   "                              HOG-Man (default)",
00047   "                              cholesky",
00048   " -i <int>                   sets the maximum number of iterations (default 10)",
00049   " -batch                     if toggled, the file is processed in offline mode",
00050   " -v                         enables the verbose mode of the optimizer",
00051   " -guiout                    dumps the output to be piped into graph_viewer",
00052   " -guess                     perform initial guess (batch mode)",
00053   " -o <filename>              writes in <filename> the optimized graph",
00054   " -oc                        overwrite the covariances with the identity",
00055   " -h                         this help",
00056   0
00057 };
00058 
00059 void printBanner(){
00060   const char** line=banner;
00061   while (*line){
00062     cerr<< *line<< endl;
00063     line++;
00064   }
00065 }
00066 
00067 enum OptimizerType {
00068   OPT_CHOL, OPT_HCHOL
00069 };
00070 
00071 static string optimizer2string(int optimizer)
00072 {
00073   switch (optimizer) {
00074     case OPT_CHOL:
00075       return "Cholesky";
00076     case OPT_HCHOL:
00077       return "Hierarchical Cholesky (HOG-Man)";
00078     default:
00079       return "Unknown Optimizer";
00080   }
00081   return "Unknown Optimizer";
00082 }
00083 
00084 int main(int argc, char** argv)
00085 {
00086   if (argc<2){
00087     printBanner();
00088     return 0;
00089   }
00090 
00091   bool visualize = false;
00092   bool overrideCovariances = false;
00093   int iterations = 10;
00094   bool verbose = false;
00095   bool incremental = true;
00096   bool guess = 0;
00097   int optType = OPT_CHOL;
00098   int updateGraphEachN = 1;
00099   int updateVisualizationEachN = 25;
00100   char* filename = 0;
00101   char* gnudump = 0;
00102   char* outfilename = 0;
00103   GraphOptimizer3D* optimizer = 0;
00104   int numLevels = 3;
00105   int nodeDistance = 2;
00106 
00107   // command line
00108   for (int c = 1; c < argc; ++c){
00109     if (! strcmp(argv[c],"-chol")){
00110       optimizer = new CholOptimizer3D();
00111       optType = OPT_CHOL;
00112     } else if (! strcmp(argv[c],"-hchol")) {
00113       optimizer = new HCholOptimizer3D(numLevels, nodeDistance);
00114       optType = OPT_HCHOL;
00115     } else if (! strcmp(argv[c],"-v")){
00116       verbose=true;
00117     } else if (! strcmp(argv[c],"-batch")){
00118       incremental = false;
00119     } else if (! strcmp(argv[c],"-guiout")){
00120       visualize = true;
00121     } else if (! strcmp(argv[c],"-o")){
00122       c++;
00123       outfilename=argv[c];
00124     } else if (! strcmp(argv[c],"-gnudump")){
00125       c++;
00126       gnudump=argv[c];
00127     } else if (! strcmp(argv[c],"-i")){
00128       c++;
00129       iterations=atoi(argv[c]);
00130     } else if (! strcmp(argv[c],"-guess")){
00131       guess = true;
00132     } else if (! strcmp(argv[c],"-oc")){
00133       overrideCovariances = true;
00134     } else if (! strcmp(argv[c],"-h")) {
00135       printBanner();
00136       return 0;
00137     } else {
00138       filename=argv[c];
00139     }
00140   }
00141 
00142   if (!optimizer) {
00143     optimizer = new HCholOptimizer3D(numLevels, nodeDistance);
00144     optType = OPT_HCHOL;
00145   }
00146 
00147   ifstream is(filename);
00148   if (!is) {
00149     cerr << "Error opening " << filename << endl;
00150     return 1;
00151   } 
00152 
00153   cerr << "# Optimizer started, Parameter summary:" << endl;
00154   cerr << "# strategy=      " << optimizer2string(optType) << endl;
00155   cerr << "# verbose=       " << verbose << endl;
00156   cerr << "# iterations=    " << iterations << endl;
00157   cerr << "# verbose=       " << verbose << endl;
00158   cerr << "# outfile=       " << ((outfilename)? outfilename : "not set") << endl;
00159   cerr << "# infile=        " << ((filename)? filename : "not set") << endl;
00160   cerr << "# incemental=    " << incremental << endl;
00161   cerr << "# initial guess= " << guess << endl;
00162 
00163   // set the optimizer setting
00164   optimizer->verbose() = verbose;
00165   optimizer->visualizeToStdout() = visualize;
00166   optimizer->guessOnEdges() = incremental;
00167 
00168   if (incremental) {
00169     optimizer->visualizeToStdout() = false;
00170     optimizer->verbose() = false;
00171 
00172     cerr << "# Loading Edges ... ";
00173     LoadedEdgeSet3D loadedEdges;
00174     loadEdges3D(loadedEdges, is, overrideCovariances);
00175     cerr << "done." << endl;
00176 
00177     struct timeval ts, te;
00178     double cumTime=0;
00179     bool addNextEdge=true;
00180     bool freshlyOptimized=false;
00181     int count=0;
00182     for (LoadedEdgeSet3D::const_iterator it = loadedEdges.begin(); it != loadedEdges.end(); ++it) {
00183       bool optimize=false;
00184 
00185       if (addNextEdge && !optimizer->vertices().empty()){
00186         int maxInGraph = optimizer->vertices().rbegin()->first;
00187         int idMax = max(it->id1, it->id2);
00188         if (maxInGraph < idMax && ! freshlyOptimized){
00189           addNextEdge=false;
00190           optimize=true;
00191           count++;
00192         } else {
00193           addNextEdge=true;
00194           optimize=false;
00195         }
00196       }
00197 
00198       PoseGraph3D::Vertex* v1 = optimizer->vertex(it->id1);
00199       PoseGraph3D::Vertex* v2 = optimizer->vertex(it->id2);
00200       if (! v1 && addNextEdge) {
00201         //cerr << " adding vertex " << it->id1 << endl;
00202         v1 = optimizer->addVertex(it->id1, Transformation3(), Matrix6::eye(1.0));
00203         assert(v1);
00204       }
00205 
00206       if (! v2 && addNextEdge) {
00207         //cerr << " adding vertex " << it->id2 << endl;
00208         v2 = optimizer->addVertex(it->id2, Transformation3(), Matrix6::eye(1.0));
00209         assert(v2);
00210       }
00211 
00212       if (addNextEdge){
00213         //cerr << " adding edge " << it->id1 <<  " " << it->id2 << " " << it->mean << endl;
00214         optimizer->addEdge(v1, v2, it->mean, it->informationMatrix);
00215       }
00216 
00217       freshlyOptimized=false;
00218       if (optimize){
00219         //cerr << "Optimize" << endl;
00220         if (!(count % updateGraphEachN)){
00221           gettimeofday(&ts, 0);
00222           int currentIt=optimizer->optimize(iterations, true);
00223           gettimeofday(&te,0);
00224           double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec);
00225           cumTime += dts;
00226           //optimizer->setOptimizationTime(cumTime);
00227           if (verbose) {
00228             double chi2 = optimizer->chi2();
00229             cerr << "nodes= " << optimizer->vertices().size() << "\t edges= " << optimizer->edges().size() << "\t chi2= " << chi2
00230               << "\t time= " << dts << "\t iterations= " << currentIt <<  "\t cumTime= " << cumTime << endl;
00231           }
00232         }
00233 
00234         // update visualization
00235         if (visualize && !(count % updateVisualizationEachN) ) {
00236           HCholOptimizer3D* opt=dynamic_cast<HCholOptimizer3D*>(optimizer);
00237           if (0 && opt) {
00238             opt=opt->level(opt->nLevels()-1);
00239             opt->visualizeToStream(cout);
00240           } else {
00241             optimizer->visualizeToStream(cout);
00242           }
00243         }
00244 
00245         if (! verbose)
00246           cerr << ".";
00247         addNextEdge=true;
00248         freshlyOptimized=true;
00249         it--;
00250         count++;
00251       }
00252     } // for all edges
00253 
00254     if (visualize) { // visualize the final state
00255       optimizer->visualizeToStream(cout);
00256     }
00257 
00258     cerr << "**** Optimization Done ****" << endl;
00259     cerr << "TOTAL TIME= " << cumTime << " s." << endl;
00260     cerr << "# final chi=" << optimizer->chi2() << endl;
00261 
00262   } else {
00263     optimizer->guessOnEdges() = guess;
00264 
00265     struct timeval ts, te;
00266     optimizer->load(is, overrideCovariances);
00267     if (! optimizer->initialize(0)){
00268       cerr << "error in initialization" << endl;
00269     }
00270     cerr << "# initial chi=" << optimizer->chi2() << endl;
00271 
00272     gettimeofday(&ts,0);
00273     optimizer->optimize(iterations, false);
00274     gettimeofday(&te,0);
00275     cerr << "**** Optimization Done ****" << endl;
00276     double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec);
00277     cerr << "TOTAL TIME= " << dts << " s." << endl;
00278     cerr << "# final chi=" << optimizer->chi2() << endl;
00279   }
00280 
00281   if (outfilename) {
00282     cerr << "Saving Graph to " << outfilename << " ... ";
00283     ofstream fout(outfilename);
00284     optimizer->save(fout);
00285     fout.close();
00286     cerr << "done." << endl;
00287   }
00288 
00289   if (gnudump) {
00290     cerr << "Saving Data to " << gnudump << " ... ";
00291     ofstream fout(gnudump);
00292     optimizer->saveGnuplot(fout);
00293     fout.close();
00294     cerr << "done." << endl;
00295   }
00296 
00297   // clean up
00298   delete optimizer;
00299 
00300   return 0;
00301 }


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58