size_t vertexEdgesInStar(HyperGraph::EdgeSet& eset, HyperGraph::Vertex* v, Star* s, EdgeStarMap& esmap){ eset.clear(); for (HyperGraph::EdgeSet::iterator it=v->edges().begin(); it!=v->edges().end(); it++){ HyperGraph::Edge* e=*it; EdgeStarMap::iterator eit=esmap.find(e); if (eit!=esmap.end() && eit->second == s) eset.insert(e); } return eset.size(); }
int main(int argc, char** argv) { OptimizableGraph::initMultiThreading(); int maxIterations; bool verbose; string inputFilename; string gnudump; string outputfilename; string solverProperties; string strSolver; string loadLookup; bool initialGuess; bool initialGuessOdometry; bool marginalize; bool listTypes; bool listSolvers; bool listRobustKernels; bool incremental; bool guiOut; int gaugeId; string robustKernel; bool computeMarginals; bool printSolverProperties; double huberWidth; double gain; int maxIterationsWithGain; //double lambdaInit; int updateGraphEachN = 10; string statsFile; string summaryFile; bool nonSequential; // command line parsing std::vector<int> gaugeList; CommandArgs arg; arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain"); arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)"); arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.param("guess", initialGuess, false, "initial guess based on spanning tree"); arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry"); arg.param("inc", incremental, false, "run incremetally"); arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes"); arg.param("guiout", guiOut, false, "gui output while running incrementally"); arg.param("marginalize", marginalize, false, "on or off"); arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver"); arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2"); arg.param("gnudump", gnudump, "", "dump to gnuplot data file"); arg.param("robustKernel", robustKernel, "", "use this robust error function"); arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)"); arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY"); arg.param("gaugeId", gaugeId, -1, "force the gauge"); arg.param("o", outputfilename, "", "output final version of the graph"); arg.param("solver", strSolver, "gn_var", "specify which solver to use underneat\n\t {gn_var, lm_fix3_2, gn_fix6_3, lm_fix7_3}"); #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES string dummy; arg.param("solverlib", dummy, "", "specify a solver library which will be loaded"); arg.param("typeslib", dummy, "", "specify a types library which will be loaded"); #endif arg.param("stats", statsFile, "", "specify a file for the statistics"); arg.param("listTypes", listTypes, false, "list the registered types"); arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels"); arg.param("listSolvers", listSolvers, false, "list the available solvers"); 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"); arg.param("gaugeList", gaugeList, std::vector<int>(), "set the list of gauges separated by commas without spaces \n e.g: 1,2,3,4,5 "); arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument"); arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true); arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries"); arg.parseArgs(argc, argv); if (verbose) { cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl; } #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES // registering all the types from the libraries DlWrapper dlTypesWrapper; loadStandardTypes(dlTypesWrapper, argc, argv); // register all the solvers DlWrapper dlSolverWrapper; loadStandardSolver(dlSolverWrapper, argc, argv); #else if (verbose) cout << "# linked version of g2o" << endl; #endif OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance(); if (listSolvers) { solverFactory->listSolvers(cout); } if (listTypes) { Factory::instance()->printRegisteredTypes(cout, true); } if (listRobustKernels) { std::vector<std::string> kernels; RobustKernelFactory::instance()->fillKnownKernels(kernels); cout << "Robust Kernels:" << endl; for (size_t i = 0; i < kernels.size(); ++i) { cout << kernels[i] << endl; } } SparseOptimizer optimizer; optimizer.setVerbose(verbose); optimizer.setForceStopFlag(&hasToStop); SparseOptimizerTerminateAction* terminateAction = 0; if (maxIterations < 0) { cerr << "# setup termination criterion based on the gain of the iteration" << endl; maxIterations = maxIterationsWithGain; terminateAction = new SparseOptimizerTerminateAction; terminateAction->setGainThreshold(gain); terminateAction->setMaxIterations(maxIterationsWithGain); optimizer.addPostIterationAction(terminateAction); } // allocating the desired solver + testing whether the solver is okay OptimizationAlgorithmProperty solverProperty; optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty)); if (! optimizer.solver()) { cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl; return 0; } if (solverProperties.size() > 0) { bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties); if (! updateStatus) { cerr << "Failure while updating the solver properties from the given string" << endl; } } if (solverProperties.size() > 0 || printSolverProperties) { optimizer.solver()->printProperties(cerr); } // Loading the input data if (loadLookup.size() > 0) { optimizer.setRenamedTypesFromString(loadLookup); } if (inputFilename.size() == 0) { cerr << "No input data specified" << endl; return 0; } else if (inputFilename == "-") { cerr << "Read input from stdin" << endl; if (!optimizer.load(cin)) { cerr << "Error loading graph" << endl; return 2; } } else { cerr << "Read input from " << inputFilename << endl; ifstream ifs(inputFilename.c_str()); if (!ifs) { cerr << "Failed to open file" << endl; return 1; } if (!optimizer.load(ifs)) { cerr << "Error loading graph" << endl; return 2; } } cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl; cerr << "Loaded " << optimizer.edges().size() << " edges" << endl; if (optimizer.vertices().size() == 0) { cerr << "Graph contains no vertices" << endl; return 1; } set<int> vertexDimensions = optimizer.dimensions(); if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) { cerr << "The selected solver is not suitable for optimizing the given graph" << endl; return 3; } assert (optimizer.solver()); //optimizer.setMethod(str2method(strMethod)); //optimizer.setUserLambdaInit(lambdaInit); // check for vertices to fix to remove DoF bool gaugeFreedom = optimizer.gaugeFreedom(); OptimizableGraph::Vertex* gauge=0; if (gaugeList.size()){ cerr << "Fixing gauges: "; for (size_t i=0; i<gaugeList.size(); i++){ int id=gaugeList[i]; OptimizableGraph::Vertex* v=optimizer.vertex(id); if (!v){ cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting"; return -1; } else { if (i==0) gauge = v; cerr << v->id() << " "; v->setFixed(1); } } cerr << endl; gaugeFreedom = false; } else { gauge=optimizer.findGauge(); } if (gaugeFreedom) { if (! gauge) { cerr << "# cannot find a vertex to fix in this thing" << endl; return 2; } else { cerr << "# graph is fixed by node " << gauge->id() << endl; gauge->setFixed(true); } } else { cerr << "# graph is fixed by priors or already fixed vertex" << endl; } // if schur, we wanna marginalize the landmarks... if (marginalize || solverProperty.requiresMarginalize) { int maxDim = *vertexDimensions.rbegin(); int minDim = *vertexDimensions.begin(); if (maxDim != minDim) { cerr << "# Preparing Marginalization of the Landmarks ... "; for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second); if (v->dimension() != maxDim) { v->setMarginalized(true); } } cerr << "done." << endl; } } if (robustKernel.size() > 0) { AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel); cerr << "# Preparing robust error function ... "; if (creator) { if (nonSequential) { for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it); if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) { e->setRobustKernel(creator->construct()); if (huberWidth > 0) e->robustKernel()->setDelta(huberWidth); } } } else { for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it); e->setRobustKernel(creator->construct()); if (huberWidth > 0) e->robustKernel()->setDelta(huberWidth); } } cerr << "done." << endl; } else { cerr << "Unknown Robust Kernel: " << robustKernel << endl; } } // sanity check HyperDijkstra d(&optimizer); UniformCostFunction f; d.shortestPaths(gauge,&f); //cerr << PVAR(d.visited().size()) << endl; if (d.visited().size()!=optimizer.vertices().size()) { cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl; cerr << "visited: " << d.visited().size() << endl; cerr << "vertices: " << optimizer.vertices().size() << endl; } if (incremental) { cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl; cerr << CL_RED("# For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl; int incIterations = maxIterations; if (! arg.parsedParam("i")) { cerr << "# Setting default number of iterations" << endl; incIterations = 1; } int updateDisplayEveryN = updateGraphEachN; int maxDim = 0; cerr << "# incremental settings" << endl; cerr << "#\t solve every " << updateGraphEachN << endl; cerr << "#\t iterations " << incIterations << endl; SparseOptimizer::VertexIDMap vertices = optimizer.vertices(); for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second); maxDim = max(maxDim, v->dimension()); } vector<SparseOptimizer::Edge*> edges; for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it); edges.push_back(e); } optimizer.edges().clear(); optimizer.vertices().clear(); optimizer.setVerbose(false); // sort the edges in a way that inserting them makes sense sort(edges.begin(), edges.end(), IncrementalEdgesCompare()); double cumTime = 0.; int vertexCount=0; int lastOptimizedVertexCount = 0; int lastVisUpdateVertexCount = 0; bool freshlyOptimized=false; bool firstRound = true; HyperGraph::VertexSet verticesAdded; HyperGraph::EdgeSet edgesAdded; for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) { SparseOptimizer::Edge* e = *it; int doInit = 0; SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id()); SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id()); if (! v1) { SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]); bool v1Added = optimizer.addVertex(v); //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl; assert(v1Added); if (! v1Added) cerr << "Error adding vertex " << v->id() << endl; else verticesAdded.insert(v); doInit = 1; if (v->dimension() == maxDim) vertexCount++; } if (! v2) { SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]); bool v2Added = optimizer.addVertex(v); //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl; assert(v2Added); if (! v2Added) cerr << "Error adding vertex " << v->id() << endl; else verticesAdded.insert(v); doInit = 2; if (v->dimension() == maxDim) vertexCount++; } // adding the edge and initialization of the vertices { //cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl; if (! optimizer.addEdge(e)) { cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl; } else { edgesAdded.insert(e); } if (doInit) { OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); switch (doInit){ case 1: // initialize v1 from v2 { HyperGraph::VertexSet toSet; toSet.insert(to); if (e->initialEstimatePossible(toSet, from) > 0.) { //cerr << "init: " //<< to->id() << "(" << to->dimension() << ") -> " //<< from->id() << "(" << from->dimension() << ") " << endl; e->initialEstimate(toSet, from); } else { assert(0 && "Added unitialized variable to the graph"); } break; } case 2: { HyperGraph::VertexSet fromSet; fromSet.insert(from); if (e->initialEstimatePossible(fromSet, to) > 0.) { //cerr << "init: " //<< from->id() << "(" << from->dimension() << ") -> " //<< to->id() << "(" << to->dimension() << ") " << endl; e->initialEstimate(fromSet, to); } else { assert(0 && "Added unitialized variable to the graph"); } break; } default: cerr << "doInit wrong value\n"; } } } freshlyOptimized=false; { //cerr << "Optimize" << endl; if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) { if (firstRound) { if (!optimizer.initializeOptimization()){ cerr << "initialization failed" << endl; return 0; } } else { if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) { cerr << "updating initialization failed" << endl; return 0; } } verticesAdded.clear(); edgesAdded.clear(); double ts = get_monotonic_time(); int currentIt=optimizer.optimize(incIterations, !firstRound); double dts = get_monotonic_time() - ts; cumTime += dts; firstRound = false; //optimizer->setOptimizationTime(cumTime); if (verbose) { double chi2 = optimizer.chi2(); cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2 << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl; } lastOptimizedVertexCount = vertexCount; freshlyOptimized = true; if (guiOut) { if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) { dumpEdges(cout, optimizer); lastVisUpdateVertexCount = vertexCount; } } } if (! verbose) cerr << "."; } } // for all edges if (! freshlyOptimized) { double ts = get_monotonic_time(); int currentIt=optimizer.optimize(incIterations, !firstRound); double dts = get_monotonic_time() - ts; cumTime += dts; //optimizer->setOptimizationTime(cumTime); if (verbose) { double chi2 = optimizer.chi2(); cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2 << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl; } } } else { // BATCH optimization if (statsFile!=""){ // allocate buffer for statistics; optimizer.setComputeBatchStatistics(true); } optimizer.initializeOptimization(); optimizer.computeActiveErrors(); double loadChi = optimizer.chi2(); cerr << "Initial chi2 = " << FIXED(loadChi) << endl; if (initialGuess) { optimizer.computeInitialGuess(); } else if (initialGuessOdometry) { EstimatePropagatorCostOdometry costFunction(&optimizer); optimizer.computeInitialGuess(costFunction); } double initChi = optimizer.chi2(); signal(SIGINT, sigquit_handler); int result=optimizer.optimize(maxIterations); if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){ cerr << "Cholesky failed, result might be invalid" << endl; } else if (computeMarginals){ std::vector<std::pair<int, int> > blockIndices; for (size_t i=0; i<optimizer.activeVertices().size(); i++){ OptimizableGraph::Vertex* v=optimizer.activeVertices()[i]; if (v->hessianIndex()>=0){ blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex())); } if (v->hessianIndex()>0){ blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex())); } } SparseBlockMatrix<MatrixXd> spinv; if (optimizer.computeMarginals(spinv, blockIndices)) { for (size_t i=0; i<optimizer.activeVertices().size(); i++){ OptimizableGraph::Vertex* v=optimizer.activeVertices()[i]; cerr << "Vertex id:" << v->id() << endl; if (v->hessianIndex()>=0){ cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl; cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex())); cerr << endl; } if (v->hessianIndex()>0){ cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl; cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex())); cerr << endl; } } } } optimizer.computeActiveErrors(); double finalChi=optimizer.chi2(); if (summaryFile!="") { PropertyMap summary; summary.makeProperty<StringProperty>("filename", inputFilename); summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size()); summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size()); int nLandmarks=0; int nPoses=0; int maxDim = *vertexDimensions.rbegin(); for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second); if (v->dimension() != maxDim) { nLandmarks++; } else nPoses++; } set<string> edgeTypes; for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){ edgeTypes.insert(Factory::instance()->tag(*it)); } stringstream edgeTypesString; for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){ edgeTypesString << *it << " "; } summary.makeProperty<IntProperty>("n_poses", nPoses); summary.makeProperty<IntProperty>("n_landmarks", nLandmarks); summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str()); summary.makeProperty<DoubleProperty>("load_chi", loadChi); summary.makeProperty<StringProperty>("solver", strSolver); summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0); summary.makeProperty<DoubleProperty>("init_chi", initChi); summary.makeProperty<DoubleProperty>("final_chi", finalChi); summary.makeProperty<IntProperty>("maxIterations", maxIterations); summary.makeProperty<IntProperty>("realIterations", result); ofstream os; os.open(summaryFile.c_str(), ios::app); summary.writeToCSV(os); } if (statsFile!=""){ cerr << "writing stats to file \"" << statsFile << "\" ... "; ofstream os(statsFile.c_str()); const BatchStatisticsContainer& bsc = optimizer.batchStatistics(); for (int i=0; i<maxIterations; i++) { os << bsc[i] << endl; } cerr << "done." << endl; } } // saving again if (gnudump.size() > 0) { bool gnuPlotStatus = saveGnuplot(gnudump, optimizer); if (! gnuPlotStatus) { cerr << "Error while writing gnuplot files" << endl; } } if (outputfilename.size() > 0) { if (outputfilename == "-") { cerr << "saving to stdout"; optimizer.save(cout); } else { cerr << "saving " << outputfilename << " ... "; optimizer.save(outputfilename.c_str()); } cerr << "done." << endl; } // destroy all the singletons //Factory::destroy(); //OptimizationAlgorithmFactory::destroy(); //HyperGraphActionLibrary::destroy(); return 0; }