00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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
00208 v2=optimizer->addVertex(it->id2, Transformation2(), Matrix3::eye(1.));
00209 assert(v2);
00210 }
00211
00212 if (addNextEdge){
00213
00214 optimizer->addEdge(v1, v2, it->mean, it->informationMatrix);
00215 }
00216
00217 freshlyOptimized=false;
00218 if (optimize){
00219
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 }
