g2o.cpp File Reference

#include <signal.h>
#include <algorithm>
#include <cassert>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include "dl_wrapper.h"
#include "g2o/config.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/factory.h"
#include "g2o/core/hyper_dijkstra.h"
#include "g2o/core/hyper_graph_action.h"
#include "g2o/core/optimization_algorithm.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/robust_kernel.h"
#include "g2o/core/robust_kernel_factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/sparse_optimizer_terminate_action.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.h"
#include "g2o_common.h"
#include "output_helper.h"

Include dependency graph for g2o.cpp:

Go to the source code of this file.

Classes
struct	IncrementalEdgesCompare
	Sort Edges for inserting them sequentially. More...

Functions
void	sigquit_handler (int sig)
int	main (int argc, char **argv)

Variables
static bool	hasToStop = false

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 100 of file g2o.cpp.

                                {
  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");
  arg.param("ig", maxIterationsWithGain, std::numeric_limits<int>::max(),
            "Maximum number of iterations with gain enabled");
  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);
 
  if (maxIterations < 0) {
    cerr << "# setup termination criterion based on the gain of the iteration"
         << endl;
    maxIterations = maxIterationsWithGain;
    SparseOptimizerTerminateAction* 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 uninitialized 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 uninitialized 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;
}

References g2o::SparseOptimizer::activeVertices(), g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addPostIterationAction(), g2o::OptimizableGraph::addVertex(), g2o::SparseOptimizer::batchStatistics(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::OptimizableGraph::chi2(), CL_RED, g2o::SparseOptimizer::computeActiveErrors(), g2o::SparseOptimizer::computeInitialGuess(), g2o::SparseOptimizer::computeMarginals(), g2o::AbstractRobustKernelCreator::construct(), g2o::OptimizationAlgorithmFactory::construct(), g2o::RobustKernelFactory::creator(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::dimensions(), g2o::dumpEdges(), g2o::HyperGraph::edges(), g2o::RobustKernelFactory::fillKnownKernels(), g2o::SparseOptimizer::findGauge(), g2o::SparseOptimizer::gaugeFreedom(), g2o::get_monotonic_time(), hasToStop, g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::HyperGraph::Vertex::id(), g2o::SparseOptimizer::initializeOptimization(), g2o::OptimizableGraph::initMultiThreading(), g2o::Factory::instance(), g2o::OptimizationAlgorithmFactory::instance(), g2o::RobustKernelFactory::instance(), g2o::OptimizableGraph::isSolverSuitable(), g2o::OptimizationAlgorithmFactory::listSolvers(), g2o::OptimizableGraph::load(), g2o::loadStandardSolver(), g2o::loadStandardTypes(), g2o::PropertyMap::makeProperty(), g2o::SparseOptimizer::optimize(), g2o::CommandArgs::param(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::CommandArgs::parsedParam(), g2o::OptimizationAlgorithm::printProperties(), g2o::Factory::printRegisteredTypes(), g2o::OptimizationAlgorithmProperty::requiresMarginalize, g2o::OptimizableGraph::save(), g2o::saveGnuplot(), g2o::SparseOptimizer::setAlgorithm(), g2o::SparseOptimizer::setComputeBatchStatistics(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::SparseOptimizer::setForceStopFlag(), g2o::SparseOptimizerTerminateAction::setGainThreshold(), g2o::OptimizableGraph::Vertex::setMarginalized(), g2o::SparseOptimizerTerminateAction::setMaxIterations(), g2o::OptimizableGraph::setRenamedTypesFromString(), g2o::SparseOptimizer::setVerbose(), g2o::HyperDijkstra::shortestPaths(), sigquit_handler(), g2o::SparseOptimizer::solver(), g2o::Factory::tag(), g2o::SparseOptimizer::updateInitialization(), g2o::OptimizationAlgorithm::updatePropertiesFromString(), g2o::OptimizableGraph::vertex(), g2o::HyperGraph::Edge::vertex(), g2o::HyperGraph::Edge::vertices(), g2o::HyperGraph::vertices(), g2o::HyperDijkstra::visited(), and g2o::PropertyMap::writeToCSV().

sigquit_handler()

void sigquit_handler

(

int

sig

)

Definition at line 89 of file g2o.cpp.

                              {
  if (sig == SIGINT) {
    hasToStop = 1;
    static int cnt = 0;
    if (cnt++ == 2) {
      cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
      exit(1);
    }
  }
}

References __PRETTY_FUNCTION__, and hasToStop.

Referenced by main().

Variable Documentation

hasToStop

bool hasToStop = false

static

Definition at line 59 of file g2o.cpp.

Referenced by main(), and sigquit_handler().