hogman2d.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 <hogman_minimal/graph_optimizer/graph_optimizer2d_aux.h>
00018 #include "hogman_minimal/graph_optimizer_hogman/graph_optimizer2d_chol.h"
00019 #include "hogman_minimal/graph_optimizer_hogman/graph_optimizer2d_hchol.h"
00020 
00021 #include <cstdlib>
00022 #include <assert.h>
00023 #include <string.h> 
00024 #include <iostream>
00025 #include <fstream>
00026 #include <sstream>
00027 #include <sys/time.h>
00028 #include <hogman_minimal/stuff/os_specific.h>
00029 #include <hogman_minimal/stuff/filesys_tools.h>
00030 #include <hogman_minimal/stuff/string_tools.h>
00031 
00032 using namespace std;
00033 using namespace AISNavigation;
00034 
00035 
00036 const char * banner[]={
00037   "*******************************************************************",
00038   "*                           HOG-Man v 0.1                         *",
00039   "*              (c) Giorgio Grisetti, Rainer Kuemmerle,            *",
00040   "*                  Cyrill Stachniss                               *",
00041   "*******************************************************************",
00042   "",
00043   "usage: hogman2d [options] <graph_file> ",
00044   "",
00045   "options:",
00046   " -hogman | -chol            selects the optimization strategy",
00047   "                            between",
00048   "                              HOG-Man (default)",
00049   "                              cholesky",
00050   " -nomanifold                disables the manifold",
00051   " -i <int>                   sets the maximum number of iterations (default 10)",
00052   " -batch                     if toggled, the file is processed in offline mode",
00053   " -v                         enables the verbose mode of the optimizer",
00054   " -gnuout                    dumps the output to be piped into gnuplot",
00055   " -guess                     perform initial guess (batch mode)",
00056   " -o <filename>              writes in <filename> the optimized graph",
00057   " -h                         this help",
00058   0
00059 };
00060 
00061 void printBanner(){
00062   const char** line=banner;
00063   while(*line){
00064     cerr<< *line<< endl;
00065     line++;
00066   }
00067 }
00068 
00069 
00070 Optimizer2D* optimizer=0;
00071 
00072 int main (int argc, char** argv){
00073   if (argc<2){
00074     printBanner();
00075     return 0;
00076   }
00077 
00078   enum OptType {hchol,chol};
00079   OptType optType = chol;
00080 
00081   const char* filename=0;
00082   const char* outfilename=0;
00083   const char* gnudump=0;
00084   int iterations=10;
00085   bool gnuout=false;
00086   bool verbose=false;
00087   bool incremental=true;
00088   bool useManifold=true;
00089   bool guess = false;
00090   int numLevels = 3;
00091   int nodeDistance = 2;
00092   int c=1;
00093   while(c<argc){
00094     if (! strcmp(argv[c],"-chol")){
00095       optimizer=new CholOptimizer2D;
00096       optType=chol;
00097     } else if (! strcmp(argv[c],"-hchol")){
00098       optimizer=new HCholOptimizer2D(numLevels, nodeDistance);
00099       optType=hchol;
00100     } else if (! strcmp(argv[c],"-v")){
00101       verbose=true;
00102     } else if (! strcmp(argv[c],"-batch")){
00103       incremental=false;
00104     } else if (! strcmp(argv[c],"-gnuout")){
00105       gnuout=true;
00106     } else if (! strcmp(argv[c],"-nomanifold")){
00107       useManifold=false;
00108     } else if (! strcmp(argv[c],"-o")){
00109       c++;
00110       outfilename=argv[c];
00111     } else if (! strcmp(argv[c],"-gnudump")){
00112       c++;
00113       gnudump=argv[c];
00114     } else if (! strcmp(argv[c],"-i")){
00115       c++;
00116       iterations=atoi(argv[c]);
00117     } else if (! strcmp(argv[c],"-guess")){
00118       guess = true;
00119     } else if (! strcmp(argv[c],"-h")) {
00120       printBanner();
00121       return 0;
00122     } else {
00123       filename=argv[c];
00124     }
00125     c++;
00126   }
00127   if (! filename) {
00128     cerr << "filename \"" << filename << "\" not given";
00129     return 0;
00130   }
00131   if (! optimizer) {
00132     optimizer=new HCholOptimizer2D(numLevels, nodeDistance);
00133     optType=hchol;
00134   }
00135   optimizer->verbose()=false;
00136   if (! incremental){
00137     optimizer->verbose()=verbose;
00138   }
00139   optimizer->guessOnEdges() = incremental;
00140   optimizer->visualizeToStdout() = gnuout;
00141   if (!incremental && guess)
00142     optimizer->guessOnEdges() = true;
00143 
00144   CholOptimizer2D* chold2d = dynamic_cast<CholOptimizer2D*>(optimizer);
00145   if (chold2d)
00146     chold2d->useManifold()=useManifold;
00147 
00148   ifstream is(filename);
00149   if (! is ){
00150     cerr << "error in opening the graph from file " << filename << endl;
00151     return 0;
00152   }
00153 
00154   cerr << "# Optimizer started, Parameter summary:" << endl;
00155   cerr << "# strategy=      ";
00156   switch(optType){
00157     case chol: cerr <<   "cholesky"; break;
00158     case hchol: cerr <<  "HOG-Man"; break;
00159   }
00160   cerr << endl;
00161   cerr << "# verbose=       " << verbose << endl;
00162   cerr << "# iterations=    " << iterations << endl;
00163   cerr << "# verbose=       " << verbose << endl;
00164   cerr << "# outfile=       " << ((outfilename)? outfilename : "not set") << endl;
00165   cerr << "# infile=        " << ((filename)? filename : "not set") << endl;
00166   cerr << "# incemental=    " << incremental << endl;
00167   cerr << "# initial guess= " << guess << endl;
00168   cerr << "# useManifold=   " << useManifold << endl;
00169 
00170 
00171   struct timeval ts, te;
00172   if (incremental) {
00173     optimizer->visualizeToStdout() = false;
00174     cerr << "# Loading Edges... ";
00175     LoadedEdgeSet loadedEdges;
00176     loadEdges(loadedEdges, is);
00177     cerr << "Done!" << endl;
00178     double cumTime=0;
00179     bool addNextEdge=true;
00180     bool freshlyOptimized=false;
00181     int count=0;
00182     for (LoadedEdgeSet::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=it->id1>it->id2?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       PoseGraph2D::Vertex* v1=optimizer->vertex(it->id1);
00199       if (! v1 && addNextEdge){
00200         //cerr << " adding vertex " << it->id1 << endl;
00201         v1=optimizer->addVertex(it->id1,Transformation2(), Matrix3::eye(1.));
00202         assert(v1);
00203       }
00204     
00205       PoseGraph2D::Vertex* v2=optimizer->vertex(it->id2);
00206       if (! v2 && addNextEdge){
00207         //cerr << " adding vertex " << it->id2 << endl;
00208         v2=optimizer->addVertex(it->id2, Transformation2(), Matrix3::eye(1.));
00209         assert(v2);
00210       }
00211       
00212       if (addNextEdge){
00213         //cerr << " adding edge " << it->id1 <<  " " << it->id2 << 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%1)){
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           if (verbose) {
00227             cerr << "nodes= " << optimizer->vertices().size() << "\t edges= " << optimizer->edges().size() << "\t chi2= " << optimizer->chi2() << "\t time= "
00228               << dts << "\t iterations= " << currentIt <<  "\t cumTime= " << cumTime << endl;
00229           }
00230         }
00231         if (gnuout && !(count % 10) ){
00232           optimizer->visualizeToStream(cout);
00233         }
00234         if (! verbose)
00235           cerr << ".";
00236         addNextEdge=true;
00237         freshlyOptimized=true;
00238         it--;
00239       }
00240     }
00241 
00242   } else {
00243     optimizer->load(is);
00244     for (PoseGraph2D::EdgeSet::iterator it=optimizer->edges().begin(); it!=optimizer->edges().end(); it++){
00245       PoseGraph2D::Edge* e=dynamic_cast<PoseGraph2D::Edge*>(*it);
00246       optimizer->refineEdge(e, e->mean(), e->information());
00247     }
00248     if (! optimizer->initialize(0)){
00249       cerr << "error in initialization" << endl;
00250       return 1;
00251     }
00252     cerr << "# initial chi=" << optimizer->chi2() << endl;
00253     gettimeofday(&ts,0);
00254     optimizer->optimize(iterations,false);
00255     gettimeofday(&te,0);
00256     cerr << "**** Optimization Done ****" << endl;
00257     double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec);
00258     cerr << "TOTAL TIME= " << dts << " s." << endl;
00259   }
00260 
00261   if (outfilename){
00262     ofstream os (outfilename);
00263     optimizer->save(os);
00264     os.close();
00265   }
00266   if (gnudump){
00267     ofstream os (gnudump);
00268     optimizer->saveAsGnuplot(os);
00269     os.close();
00270   }
00271 
00272   return 0;
00273 }


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