re_vision: generate_commands.cpp Source File

Go to the documentation of this file.
00001 // g2o - General Graph Optimization
00002 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
00003 // 
00004 // g2o 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 // g2o 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 <signal.h>
00018 #include <iostream>
00019 #include <iomanip>
00020 #include <string>
00021 #include <fstream>
00022 #include <sstream>
00023 #include <algorithm>
00024 #include <cassert>
00025 
00026 #include "g2o/apps/g2o_cli/dl_wrapper.h"
00027 #include "g2o/apps/g2o_cli/g2o_common.h"
00028 
00029 #include "g2o/core/estimate_propagator.h"
00030 #include "g2o/core/graph_optimizer_sparse.h"
00031 #include "g2o/core/factory.h"
00032 #include "g2o/core/solver_factory.h"
00033 #include "g2o/core/hyper_dijkstra.h"
00034 
00035 #include "g2o/stuff/macros.h"
00036 #include "g2o/stuff/color_macros.h"
00037 #include "g2o/stuff/command_args.h"
00038 #include "g2o/stuff/filesys_tools.h"
00039 #include "g2o/stuff/string_tools.h"
00040 #include "g2o/stuff/timeutil.h"
00041 
00042 static bool hasToStop=false;
00043 
00044 using namespace std;
00045 using namespace g2o;
00046 
00047 // sort according to max id, dimension, min id
00048 struct IncrementalEdgesCompare {
00049   bool operator()(SparseOptimizer::Edge* const & e1, SparseOptimizer::Edge* const & e2)
00050   {
00051     const SparseOptimizer::Vertex* to1 = static_cast<const SparseOptimizer::Vertex*>(e1->vertices()[0]);
00052     const SparseOptimizer::Vertex* to2 = static_cast<const SparseOptimizer::Vertex*>(e2->vertices()[0]);
00053 
00054     int i11 = e1->vertices()[0]->id(), i12 = e1->vertices()[1]->id();
00055     if (i11 > i12){
00056       swap(i11, i12);
00057     }
00058     int i21 = e2->vertices()[0]->id(), i22 = e2->vertices()[1]->id();
00059     if (i21 > i22){
00060       swap(i21, i22);
00061     }
00062     if (i12 < i22)
00063       return true;
00064     if (i12 > i22)
00065       return false;
00066     if (to1->dimension() != to2->dimension()) { // push the odometry to be the first
00067       return to1->dimension() > to2->dimension();
00068     }
00069     return (i11<i21);
00070   }
00071 };
00072 
00073 SparseOptimizer::Method str2method(const std::string& strMethod_){
00074   string strMethod = strToLower(strMethod_);
00075   if (strMethod=="gauss") {
00076     cerr << "# Doing Gauss" << endl;
00077     return SparseOptimizer::GaussNewton;
00078   }
00079   if (strMethod=="levenberg") {
00080     cerr << "# Doing Levenberg-Marquardt" << endl;
00081     return SparseOptimizer::LevenbergMarquardt;
00082   }
00083   cerr << "# Unknown optimization method: " << strMethod << ", setting  default to Levenberg"  << endl;
00084   return SparseOptimizer::LevenbergMarquardt;
00085 }
00086 
00087 void sigquit_handler(int sig)
00088 {
00089   if (sig == SIGINT) {
00090     hasToStop = 1;
00091     static int cnt = 0;
00092     if (cnt++ == 2) {
00093       cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
00094       exit(1);
00095     }
00096   }
00097 }
00098 
00099 int main(int argc, char** argv)
00100 {
00101   int maxIterations;
00102   bool verbose;
00103   string inputFilename;
00104   string gnudump;
00105   string outputfilename;
00106   string strMethod;
00107   string strSolver;
00108   string loadLookup;
00109   bool initialGuess;
00110   bool marginalize;
00111   bool listTypes;
00112   bool listSolvers;
00113   bool incremental;
00114   bool guiOut;
00115   bool robustKernel;
00116   double huberWidth;
00117   double lambdaInit;
00118   int updateGraphEachN = 10;
00119   string statsFile;
00120   string dummy;
00121   // command line parsing
00122   CommandArgs arg;
00123   arg.param("i", maxIterations, 5, "perform n iterations");
00124   arg.param("v", verbose, false, "verbose output of the optimization process");
00125   arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
00126   arg.param("inc", incremental, false, "run incremetally");
00127   arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes, (default: 10)");
00128   arg.param("guiout", guiOut, false, "gui output while running incrementally");
00129   arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
00130   arg.param("marginalize", marginalize, false, "on or off");
00131   arg.param("method", strMethod, "Gauss", "Gauss or Levenberg");
00132   arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
00133   arg.param("robustKernel", robustKernel, false, "use robust error functions");
00134   arg.param("huberWidth", huberWidth, -1., "width for the robust Huber Kernel (only if robustKernel)");
00135   arg.param("o", outputfilename, "", "output final version of the graph");
00136   arg.param("solver", strSolver, "var", "specify which solver to use underneat\n\t {var, fix3_2, fix6_3, fix_7_3}");
00137   arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
00138   arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
00139   arg.param("stats", statsFile, "", "specify a file for the statistics");
00140   arg.param("listTypes", listTypes, false, "list the registered types");
00141   arg.param("listSolvers", listSolvers, false, "list the available solvers");
00142   arg.param("renameTypes", loadLookup, "", "create a lookup for loading types into other types,\n\t TAG_IN_FILE=INTERNAL_TAG_FOR_TYPE,TAG2=INTERNAL2\n\t e.g., VERTEX_CAM=VERTEX_SE3:EXPMAP");
00143   arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
00144 
00145   arg.parseArgs(argc, argv);
00146 
00147   // registering all the types from the libraries
00148   DlWrapper dlTypesWrapper;
00149   loadStandardTypes(dlTypesWrapper, argc, argv);
00150 
00151   // register all the solvers
00152   //SolverFactory* solverFactory = SolverFactory::instance();
00153   //DlWrapper dlSolverWrapper;
00154   //loadStandardSolver(dlSolverWrapper, argc, argv);
00155   //if (listSolvers)
00156     //solverFactory->listSolvers(cerr);
00157 
00158   if (listTypes) {
00159     Factory::instance()->printRegisteredTypes(cout, true);
00160   }
00161 
00162   SparseOptimizer optimizer;
00163   //optimizer.setVerbose(verbose);
00164   //optimizer.setForceStopFlag(&hasToStop);
00165 
00166   // Loading the input data
00167   if (loadLookup.size() > 0) {
00168     optimizer.setRenamedTypesFromString(loadLookup);
00169   }
00170   if (inputFilename.size() == 0) {
00171     cerr << "No input data specified" << endl;
00172     return 0;
00173   } else if (inputFilename == "-") {
00174     cerr << "Read input from stdin" << endl;
00175     if (!optimizer.load(cin)) {
00176       cerr << "Error loading graph" << endl;
00177       return 2;
00178     }
00179   } else {
00180     cerr << "Read input from " << inputFilename << endl;
00181     ifstream ifs(inputFilename.c_str());
00182     if (!ifs) {
00183       cerr << "Failed to open file" << endl;
00184       return 1;
00185     }
00186     if (!optimizer.load(ifs)) {
00187       cerr << "Error loading graph" << endl;
00188       return 2;
00189     }
00190   }
00191   cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
00192   cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
00193 
00194   if (optimizer.vertices().size() == 0) {
00195     cerr << "Graph contains no vertices" << endl;
00196     return 1;
00197   }
00198 
00199   if (1) {
00200     int incIterations = maxIterations;
00201     int maxDim = 0;
00202 
00203     cerr << "# incremental setttings" << endl;
00204     cerr << "#\t solve every " << updateGraphEachN << endl;
00205     cerr << "#\t iterations  " << incIterations << endl;
00206 
00207     SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
00208     for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
00209       const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
00210       maxDim = max(maxDim, v->dimension());
00211     }
00212 
00213     vector<SparseOptimizer::Edge*> edges;
00214     for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
00215       SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
00216       edges.push_back(e);
00217     }
00218     optimizer.edges().clear();
00219     optimizer.vertices().clear();
00220     optimizer.setVerbose(false);
00221 
00222     // sort the edges in a way that inserting them makes sense
00223     sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
00224 
00225     int vertexCount=0;
00226     int lastOptimizedVertexCount = 0;
00227     bool addNextEdge=true;
00228     bool freshlyOptimized=false;
00229     HyperGraph::VertexSet verticesAdded;
00230     int maxInGraph = -1;
00231     for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
00232       SparseOptimizer::Edge* e = *it;
00233       bool optimize=false;
00234 
00235       if (addNextEdge && !optimizer.vertices().empty()){
00236         int idMax = max(e->vertices()[0]->id(), e->vertices()[1]->id());
00237         if (maxInGraph < idMax && ! freshlyOptimized){
00238           addNextEdge=false;
00239           optimize=true;
00240         } else {
00241           addNextEdge=true;
00242           optimize=false;
00243         }
00244       }
00245 
00246       int doInit = 0;
00247       SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
00248       SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
00249       if (! v1 && addNextEdge) {
00250         //cerr << " adding vertex " << it->id1 << endl;
00251         SparseOptimizer::Vertex* v = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
00252         bool v1Added = optimizer.addVertex(v);
00253         maxInGraph = max(maxInGraph, v->id());
00254         //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
00255         assert(v1Added);
00256         if (! v1Added)
00257           cerr << "Error adding vertex " << v->id() << endl;
00258         else
00259           verticesAdded.insert(v);
00260         doInit = 1;
00261         if (v->dimension() == maxDim)
00262           vertexCount++;
00263 
00264         if (v->dimension() == 3) {
00265           cout << "ADD VERTEX_XYT " << v->id() << ";" << endl;
00266         }
00267         else if (v->dimension() == 6) {
00268           cout << "ADD VERTEX_XYZRPY " << v->id() << ";" << endl;
00269         }
00270 
00271       }
00272 
00273       if (! v2 && addNextEdge) {
00274         SparseOptimizer::Vertex* v = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
00275         //cerr << " adding vertex " << v->id() << endl;
00276         bool v2Added = optimizer.addVertex(v);
00277         maxInGraph = max(maxInGraph, v->id());
00278         //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
00279         assert(v2Added);
00280         if (! v2Added)
00281           cerr << "Error adding vertex " << v->id() << endl;
00282         else
00283           verticesAdded.insert(v);
00284         doInit = 2;
00285         if (v->dimension() == maxDim)
00286           vertexCount++;
00287         
00288         if (v->dimension() == 3) {
00289           cout << "ADD VERTEX_XYT " << v->id() << ";" << endl;
00290         }
00291         else if (v->dimension() == 6) {
00292           cout << "ADD VERTEX_XYZRPY " << v->id() << ";" << endl;
00293         }
00294       }
00295 
00296       if (addNextEdge){
00297 
00298         static int edgeCnt = 0;
00299        
00300         if (e->dimension() == 3) {
00301           double* information = e->informationData();
00302           double meas[3];
00303           e->getMeasurementData(meas);
00304           //ADD EDGE_XYT 1 1 2 .1 .2 .3 1 0 0 1 0 1;
00305           cout << "ADD EDGE_XYT " << edgeCnt++ << " " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << " "
00306             << meas[0] << " " << meas[1] << " " << meas[2];
00307           for (int i = 0; i < 3; ++i)
00308             for (int j = i; j < 3; ++j)
00309               cout << " " << information[i*3 + j];
00310           cout << ";" << endl;
00311         }
00312         else if (e->dimension() == 6) {
00313           // TODO convert to EULER angles
00314           cerr << "NOT IMPLEMENTED YET" << endl;
00315         }
00316         static bool firstEdge = true;
00317         if (firstEdge) {
00318           firstEdge = false;
00319           cout << "FIX 0;" << endl;
00320         }
00321 
00322         //cerr << " adding edge " << e->vertices()[0]->id() <<  " " << e->vertices()[1]->id() << endl;
00323         if (! optimizer.addEdge(e)) {
00324           cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
00325         } 
00326       }
00327 
00328       freshlyOptimized=false;
00329       if (optimize){
00330         //cerr << "Optimize" << endl;
00331         if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
00332           cout << "SOLVE_STATE;" << endl;
00333           cout << "QUERY_STATE;" << endl;
00334           lastOptimizedVertexCount = vertexCount;
00335         }
00336 
00337         addNextEdge=true;
00338         freshlyOptimized=true;
00339         it--;
00340       }
00341 
00342     } // for all edges
00343 
00344   }
00345 
00346   return 0;
00347 }