g2o
Classes | Functions | Variables
g2o.cpp File Reference
#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include "dl_wrapper.h"
#include "output_helper.h"
#include "g2o_common.h"
#include "g2o/config.h"
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/hyper_dijkstra.h"
#include "g2o/core/hyper_graph_action.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/robust_kernel.h"
#include "g2o/core/robust_kernel_factory.h"
#include "g2o/core/optimization_algorithm.h"
#include "g2o/core/sparse_optimizer_terminate_action.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.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

int main ( int  argc,
char **  argv 
)

Definition at line 102 of file g2o.cpp.

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::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::dimensions(), g2o::dumpEdges(), g2o::HyperGraph::edges(), g2o::SparseOptimizer::findGauge(), g2o::SparseOptimizer::gaugeFreedom(), g2o::get_monotonic_time(), hasToStop, g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::HyperGraph::Vertex::id(), g2o::SparseOptimizer::initializeOptimization(), 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::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::SparseOptimizer::updateInitialization(), g2o::OptimizationAlgorithm::updatePropertiesFromString(), g2o::OptimizableGraph::vertex(), vertices, g2o::HyperGraph::vertices(), g2o::HyperDijkstra::visited(), and g2o::PropertyMap::writeToCSV().

103 {
104  OptimizableGraph::initMultiThreading();
105  int maxIterations;
106  bool verbose;
107  string inputFilename;
108  string gnudump;
109  string outputfilename;
110  string solverProperties;
111  string strSolver;
112  string loadLookup;
113  bool initialGuess;
114  bool initialGuessOdometry;
115  bool marginalize;
116  bool listTypes;
117  bool listSolvers;
118  bool listRobustKernels;
119  bool incremental;
120  bool guiOut;
121  int gaugeId;
122  string robustKernel;
123  bool computeMarginals;
124  bool printSolverProperties;
125  double huberWidth;
126  double gain;
127  int maxIterationsWithGain;
128  //double lambdaInit;
129  int updateGraphEachN = 10;
130  string statsFile;
131  string summaryFile;
132  bool nonSequential;
133  // command line parsing
134  std::vector<int> gaugeList;
135  CommandArgs arg;
136  arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain");
137  arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)");
138  arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)");
139  arg.param("v", verbose, false, "verbose output of the optimization process");
140  arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
141  arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry");
142  arg.param("inc", incremental, false, "run incremetally");
143  arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes");
144  arg.param("guiout", guiOut, false, "gui output while running incrementally");
145  arg.param("marginalize", marginalize, false, "on or off");
146  arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver");
147  arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2");
148  arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
149  arg.param("robustKernel", robustKernel, "", "use this robust error function");
150  arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)");
151  arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY");
152  arg.param("gaugeId", gaugeId, -1, "force the gauge");
153  arg.param("o", outputfilename, "", "output final version of the graph");
154  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}");
155 #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
156  string dummy;
157  arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
158  arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
159 #endif
160  arg.param("stats", statsFile, "", "specify a file for the statistics");
161  arg.param("listTypes", listTypes, false, "list the registered types");
162  arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels");
163  arg.param("listSolvers", listSolvers, false, "list the available solvers");
164  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");
165  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 ");
166  arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument");
167  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
168  arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries");
169 
170 
171  arg.parseArgs(argc, argv);
172 
173  if (verbose) {
174  cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl;
175  }
176 
177 #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
178  // registering all the types from the libraries
179  DlWrapper dlTypesWrapper;
180  loadStandardTypes(dlTypesWrapper, argc, argv);
181  // register all the solvers
182  DlWrapper dlSolverWrapper;
183  loadStandardSolver(dlSolverWrapper, argc, argv);
184 #else
185  if (verbose)
186  cout << "# linked version of g2o" << endl;
187 #endif
188 
189  OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
190  if (listSolvers) {
191  solverFactory->listSolvers(cout);
192  }
193 
194  if (listTypes) {
195  Factory::instance()->printRegisteredTypes(cout, true);
196  }
197 
198  if (listRobustKernels) {
199  std::vector<std::string> kernels;
200  RobustKernelFactory::instance()->fillKnownKernels(kernels);
201  cout << "Robust Kernels:" << endl;
202  for (size_t i = 0; i < kernels.size(); ++i) {
203  cout << kernels[i] << endl;
204  }
205  }
206 
207  SparseOptimizer optimizer;
208  optimizer.setVerbose(verbose);
209  optimizer.setForceStopFlag(&hasToStop);
210 
211  SparseOptimizerTerminateAction* terminateAction = 0;
212  if (maxIterations < 0) {
213  cerr << "# setup termination criterion based on the gain of the iteration" << endl;
214  maxIterations = maxIterationsWithGain;
215  terminateAction = new SparseOptimizerTerminateAction;
216  terminateAction->setGainThreshold(gain);
217  terminateAction->setMaxIterations(maxIterationsWithGain);
218  optimizer.addPostIterationAction(terminateAction);
219  }
220 
221  // allocating the desired solver + testing whether the solver is okay
222  OptimizationAlgorithmProperty solverProperty;
223  optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty));
224  if (! optimizer.solver()) {
225  cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
226  return 0;
227  }
228 
229  if (solverProperties.size() > 0) {
230  bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties);
231  if (! updateStatus) {
232  cerr << "Failure while updating the solver properties from the given string" << endl;
233  }
234  }
235  if (solverProperties.size() > 0 || printSolverProperties) {
236  optimizer.solver()->printProperties(cerr);
237  }
238 
239  // Loading the input data
240  if (loadLookup.size() > 0) {
241  optimizer.setRenamedTypesFromString(loadLookup);
242  }
243  if (inputFilename.size() == 0) {
244  cerr << "No input data specified" << endl;
245  return 0;
246  } else if (inputFilename == "-") {
247  cerr << "Read input from stdin" << endl;
248  if (!optimizer.load(cin)) {
249  cerr << "Error loading graph" << endl;
250  return 2;
251  }
252  } else {
253  cerr << "Read input from " << inputFilename << endl;
254  ifstream ifs(inputFilename.c_str());
255  if (!ifs) {
256  cerr << "Failed to open file" << endl;
257  return 1;
258  }
259  if (!optimizer.load(ifs)) {
260  cerr << "Error loading graph" << endl;
261  return 2;
262  }
263  }
264  cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
265  cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
266 
267  if (optimizer.vertices().size() == 0) {
268  cerr << "Graph contains no vertices" << endl;
269  return 1;
270  }
271 
272  set<int> vertexDimensions = optimizer.dimensions();
273  if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
274  cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
275  return 3;
276  }
277  assert (optimizer.solver());
278  //optimizer.setMethod(str2method(strMethod));
279  //optimizer.setUserLambdaInit(lambdaInit);
280 
281 
282  // check for vertices to fix to remove DoF
283  bool gaugeFreedom = optimizer.gaugeFreedom();
284  OptimizableGraph::Vertex* gauge=0;
285  if (gaugeList.size()){
286  cerr << "Fixing gauges: ";
287  for (size_t i=0; i<gaugeList.size(); i++){
288  int id=gaugeList[i];
289  OptimizableGraph::Vertex* v=optimizer.vertex(id);
290  if (!v){
291  cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting";
292  return -1;
293  } else {
294  if (i==0)
295  gauge = v;
296  cerr << v->id() << " ";
297  v->setFixed(1);
298  }
299  }
300  cerr << endl;
301  gaugeFreedom = false;
302  } else {
303  gauge=optimizer.findGauge();
304  }
305  if (gaugeFreedom) {
306  if (! gauge) {
307  cerr << "# cannot find a vertex to fix in this thing" << endl;
308  return 2;
309  } else {
310  cerr << "# graph is fixed by node " << gauge->id() << endl;
311  gauge->setFixed(true);
312  }
313  } else {
314  cerr << "# graph is fixed by priors or already fixed vertex" << endl;
315  }
316 
317  // if schur, we wanna marginalize the landmarks...
318  if (marginalize || solverProperty.requiresMarginalize) {
319  int maxDim = *vertexDimensions.rbegin();
320  int minDim = *vertexDimensions.begin();
321  if (maxDim != minDim) {
322  cerr << "# Preparing Marginalization of the Landmarks ... ";
323  for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
324  OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
325  if (v->dimension() != maxDim) {
326  v->setMarginalized(true);
327  }
328  }
329  cerr << "done." << endl;
330  }
331  }
332 
333  if (robustKernel.size() > 0) {
334  AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel);
335  cerr << "# Preparing robust error function ... ";
336  if (creator) {
337  if (nonSequential) {
338  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
339  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
340  if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
341  e->setRobustKernel(creator->construct());
342  if (huberWidth > 0)
343  e->robustKernel()->setDelta(huberWidth);
344  }
345  }
346  } else {
347  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
348  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
349  e->setRobustKernel(creator->construct());
350  if (huberWidth > 0)
351  e->robustKernel()->setDelta(huberWidth);
352  }
353  }
354  cerr << "done." << endl;
355  } else {
356  cerr << "Unknown Robust Kernel: " << robustKernel << endl;
357  }
358  }
359 
360  // sanity check
361  HyperDijkstra d(&optimizer);
363  d.shortestPaths(gauge,&f);
364  //cerr << PVAR(d.visited().size()) << endl;
365 
366  if (d.visited().size()!=optimizer.vertices().size()) {
367  cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
368  cerr << "visited: " << d.visited().size() << endl;
369  cerr << "vertices: " << optimizer.vertices().size() << endl;
370  }
371 
372  if (incremental) {
373  cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl;
374  cerr << CL_RED("# For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl;
375  int incIterations = maxIterations;
376  if (! arg.parsedParam("i")) {
377  cerr << "# Setting default number of iterations" << endl;
378  incIterations = 1;
379  }
380  int updateDisplayEveryN = updateGraphEachN;
381  int maxDim = 0;
382 
383  cerr << "# incremental settings" << endl;
384  cerr << "#\t solve every " << updateGraphEachN << endl;
385  cerr << "#\t iterations " << incIterations << endl;
386 
387  SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
388  for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
389  const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
390  maxDim = max(maxDim, v->dimension());
391  }
392 
393  vector<SparseOptimizer::Edge*> edges;
394  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
395  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
396  edges.push_back(e);
397  }
398  optimizer.edges().clear();
399  optimizer.vertices().clear();
400  optimizer.setVerbose(false);
401 
402  // sort the edges in a way that inserting them makes sense
403  sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
404 
405  double cumTime = 0.;
406  int vertexCount=0;
407  int lastOptimizedVertexCount = 0;
408  int lastVisUpdateVertexCount = 0;
409  bool freshlyOptimized=false;
410  bool firstRound = true;
411  HyperGraph::VertexSet verticesAdded;
412  HyperGraph::EdgeSet edgesAdded;
413  for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
414  SparseOptimizer::Edge* e = *it;
415 
416  int doInit = 0;
417  SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
418  SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
419 
420  if (! v1) {
421  SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
422  bool v1Added = optimizer.addVertex(v);
423  //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
424  assert(v1Added);
425  if (! v1Added)
426  cerr << "Error adding vertex " << v->id() << endl;
427  else
428  verticesAdded.insert(v);
429  doInit = 1;
430  if (v->dimension() == maxDim)
431  vertexCount++;
432  }
433 
434  if (! v2) {
435  SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
436  bool v2Added = optimizer.addVertex(v);
437  //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
438  assert(v2Added);
439  if (! v2Added)
440  cerr << "Error adding vertex " << v->id() << endl;
441  else
442  verticesAdded.insert(v);
443  doInit = 2;
444  if (v->dimension() == maxDim)
445  vertexCount++;
446  }
447 
448  // adding the edge and initialization of the vertices
449  {
450  //cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl;
451  if (! optimizer.addEdge(e)) {
452  cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
453  } else {
454  edgesAdded.insert(e);
455  }
456 
457  if (doInit) {
458  OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
459  OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
460  switch (doInit){
461  case 1: // initialize v1 from v2
462  {
463  HyperGraph::VertexSet toSet;
464  toSet.insert(to);
465  if (e->initialEstimatePossible(toSet, from) > 0.) {
466  //cerr << "init: "
467  //<< to->id() << "(" << to->dimension() << ") -> "
468  //<< from->id() << "(" << from->dimension() << ") " << endl;
469  e->initialEstimate(toSet, from);
470  } else {
471  assert(0 && "Added unitialized variable to the graph");
472  }
473  break;
474  }
475  case 2:
476  {
477  HyperGraph::VertexSet fromSet;
478  fromSet.insert(from);
479  if (e->initialEstimatePossible(fromSet, to) > 0.) {
480  //cerr << "init: "
481  //<< from->id() << "(" << from->dimension() << ") -> "
482  //<< to->id() << "(" << to->dimension() << ") " << endl;
483  e->initialEstimate(fromSet, to);
484  } else {
485  assert(0 && "Added unitialized variable to the graph");
486  }
487  break;
488  }
489  default: cerr << "doInit wrong value\n";
490  }
491 
492  }
493 
494  }
495 
496  freshlyOptimized=false;
497  {
498  //cerr << "Optimize" << endl;
499  if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
500  if (firstRound) {
501  if (!optimizer.initializeOptimization()){
502  cerr << "initialization failed" << endl;
503  return 0;
504  }
505  } else {
506  if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) {
507  cerr << "updating initialization failed" << endl;
508  return 0;
509  }
510  }
511  verticesAdded.clear();
512  edgesAdded.clear();
513  double ts = get_monotonic_time();
514  int currentIt=optimizer.optimize(incIterations, !firstRound);
515  double dts = get_monotonic_time() - ts;
516  cumTime += dts;
517  firstRound = false;
518  //optimizer->setOptimizationTime(cumTime);
519  if (verbose) {
520  double chi2 = optimizer.chi2();
521  cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
522  << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
523  }
524  lastOptimizedVertexCount = vertexCount;
525  freshlyOptimized = true;
526 
527  if (guiOut) {
528  if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) {
529  dumpEdges(cout, optimizer);
530  lastVisUpdateVertexCount = vertexCount;
531  }
532  }
533 
534  }
535 
536  if (! verbose)
537  cerr << ".";
538  }
539 
540  } // for all edges
541 
542  if (! freshlyOptimized) {
543  double ts = get_monotonic_time();
544  int currentIt=optimizer.optimize(incIterations, !firstRound);
545  double dts = get_monotonic_time() - ts;
546  cumTime += dts;
547  //optimizer->setOptimizationTime(cumTime);
548  if (verbose) {
549  double chi2 = optimizer.chi2();
550  cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
551  << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
552  }
553 
554  }
555 
556  } else {
557 
558  // BATCH optimization
559 
560  if (statsFile!=""){
561  // allocate buffer for statistics;
562  optimizer.setComputeBatchStatistics(true);
563  }
564  optimizer.initializeOptimization();
565  optimizer.computeActiveErrors();
566  double loadChi = optimizer.chi2();
567  cerr << "Initial chi2 = " << FIXED(loadChi) << endl;
568 
569  if (initialGuess) {
570  optimizer.computeInitialGuess();
571  } else if (initialGuessOdometry) {
572  EstimatePropagatorCostOdometry costFunction(&optimizer);
573  optimizer.computeInitialGuess(costFunction);
574  }
575  double initChi = optimizer.chi2();
576 
577  signal(SIGINT, sigquit_handler);
578  int result=optimizer.optimize(maxIterations);
579  if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){
580  cerr << "Cholesky failed, result might be invalid" << endl;
581  } else if (computeMarginals){
582  std::vector<std::pair<int, int> > blockIndices;
583  for (size_t i=0; i<optimizer.activeVertices().size(); i++){
584  OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
585  if (v->hessianIndex()>=0){
586  blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
587  }
588  if (v->hessianIndex()>0){
589  blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
590  }
591  }
593  if (optimizer.computeMarginals(spinv, blockIndices)) {
594  for (size_t i=0; i<optimizer.activeVertices().size(); i++){
595  OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
596  cerr << "Vertex id:" << v->id() << endl;
597  if (v->hessianIndex()>=0){
598  cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
599  cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex()));
600  cerr << endl;
601  }
602  if (v->hessianIndex()>0){
603  cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
604  cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
605  cerr << endl;
606  }
607  }
608  }
609  }
610 
611  optimizer.computeActiveErrors();
612  double finalChi=optimizer.chi2();
613 
614  if (summaryFile!="") {
615  PropertyMap summary;
616  summary.makeProperty<StringProperty>("filename", inputFilename);
617  summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size());
618  summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size());
619 
620  int nLandmarks=0;
621  int nPoses=0;
622  int maxDim = *vertexDimensions.rbegin();
623  for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
624  OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
625  if (v->dimension() != maxDim) {
626  nLandmarks++;
627  } else
628  nPoses++;
629  }
630  set<string> edgeTypes;
631  for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){
632  edgeTypes.insert(Factory::instance()->tag(*it));
633  }
634  stringstream edgeTypesString;
635  for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){
636  edgeTypesString << *it << " ";
637  }
638 
639  summary.makeProperty<IntProperty>("n_poses", nPoses);
640  summary.makeProperty<IntProperty>("n_landmarks", nLandmarks);
641  summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str());
642  summary.makeProperty<DoubleProperty>("load_chi", loadChi);
643  summary.makeProperty<StringProperty>("solver", strSolver);
644  summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0);
645  summary.makeProperty<DoubleProperty>("init_chi", initChi);
646  summary.makeProperty<DoubleProperty>("final_chi", finalChi);
647  summary.makeProperty<IntProperty>("maxIterations", maxIterations);
648  summary.makeProperty<IntProperty>("realIterations", result);
649  ofstream os;
650  os.open(summaryFile.c_str(), ios::app);
651  summary.writeToCSV(os);
652  }
653 
654 
655  if (statsFile!=""){
656  cerr << "writing stats to file \"" << statsFile << "\" ... ";
657  ofstream os(statsFile.c_str());
658  const BatchStatisticsContainer& bsc = optimizer.batchStatistics();
659 
660  for (int i=0; i<maxIterations; i++) {
661  os << bsc[i] << endl;
662  }
663  cerr << "done." << endl;
664  }
665 
666  }
667 
668  // saving again
669  if (gnudump.size() > 0) {
670  bool gnuPlotStatus = saveGnuplot(gnudump, optimizer);
671  if (! gnuPlotStatus) {
672  cerr << "Error while writing gnuplot files" << endl;
673  }
674  }
675 
676  if (outputfilename.size() > 0) {
677  if (outputfilename == "-") {
678  cerr << "saving to stdout";
679  optimizer.save(cout);
680  } else {
681  cerr << "saving " << outputfilename << " ... ";
682  optimizer.save(outputfilename.c_str());
683  }
684  cerr << "done." << endl;
685  }
686 
687  // destroy all the singletons
688  //Factory::destroy();
689  //OptimizationAlgorithmFactory::destroy();
690  //HyperGraphActionLibrary::destroy();
691 
692  return 0;
693 }
virtual bool updateInitialization(HyperGraph::VertexSet &vset, HyperGraph::EdgeSet &eset)
virtual void computeInitialGuess()
double get_monotonic_time()
Definition: timeutil.cpp:113
const BatchStatisticsContainer & batchStatistics() const
int id() const
returns the id
Definition: hyper_graph.h:148
Command line parsing of argc and argv.
Definition: command_args.h:46
Abstract interface for allocating a robust kernel.
describe the properties of a solver
void setComputeBatchStatistics(bool computeBatchStatistics)
bool dumpEdges(std::ostream &os, const OptimizableGraph &optimizer)
void loadStandardTypes(DlWrapper &dlTypesWrapper, int argc, char **argv)
Definition: g2o_common.cpp:96
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:136
int optimize(int iterations, bool online=false)
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
#define CL_RED(s)
Definition: color_macros.h:46
stop iterating based on the gain which is (oldChi - currentChi) / currentChi.
OptimizationAlgorithm * solver()
int hessianIndex() const
temporary index of this node in the parameter vector obtained from linearization
bool addPostIterationAction(HyperGraphAction *action)
add an action to be executed after each iteration
static bool hasToStop
Definition: g2o.cpp:60
create solvers based on their short name
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
Vertex * vertex(int id)
returns the vertex number id appropriately casted
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:135
virtual Vertex * findGauge()
finds a gauge in the graph to remove the undefined dof.
int dimension() const
dimension of the estimated state belonging to this node
Protocol The SLAM executable accepts such as solving the and retrieving or vertices in the explicitly state the reprensentation poses are represented by poses by VERTEX_XYZRPY In the Quaternions and other representations could be but note that it is up to the SLAM algorithm to choose the internal representation of the angles The keyword is followed by a unique vertex ID and an optional initialization of the or edges in the explicitly state the type of the constraint pose constraints are given by pose constraints by EDGE_XYZRPY The keyword is followed by a unique edge the IDs of the referenced vertices
Definition: protocol.txt:7
P * makeProperty(const std::string &name_, const typename P::ValueType &v)
Definition: property.h:119
double chi2() const
returns the chi2 of the current configuration
bool parseArgs(int argc, char **argv, bool exitOnError=true)
virtual bool save(std::ostream &os, int level=0) const
save the graph to a stream. Again uses the Factory system.
bool isSolverSuitable(const OptimizationAlgorithmProperty &solverProperty, const std::set< int > &vertDims=std::set< int >()) const
void setVerbose(bool verbose)
const EdgeSet & edges() const
Definition: hyper_graph.h:230
void setForceStopFlag(bool *flag)
void paramLeftOver(const std::string &name, std::string &p, const std::string &defValue, const std::string &desc, bool optional=false)
void setAlgorithm(OptimizationAlgorithm *algorithm)
bool updatePropertiesFromString(const std::string &propString)
void param(const std::string &name, bool &p, bool defValue, const std::string &desc)
void writeToCSV(std::ostream &os) const
Definition: property.cpp:74
void listSolvers(std::ostream &os) const
list the known solvers into a stream
void loadStandardSolver(DlWrapper &dlSolverWrapper, int argc, char **argv)
Definition: g2o_common.cpp:133
A general case Vertex for optimization.
Loading libraries during run-time.
Definition: dl_wrapper.h:44
void setRenamedTypesFromString(const std::string &types)
virtual bool load(std::istream &is, bool createEdges=true)
load the graph from a stream. Uses the Factory singleton for creating the vertices and edges...
const VertexContainer & activeVertices() const
the vertices active in the current optimization
void setMarginalized(bool marginalized)
true => this node should be marginalized out during the optimization
Sort Edges for inserting them sequentially.
Definition: g2o.cpp:67
bool saveGnuplot(const std::string &gnudump, const OptimizableGraph &optimizer)
bool requiresMarginalize
whether the solver requires marginalization of landmarks
virtual RobustKernel * construct()=0
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
cost for traversing only odometry edges.
void printProperties(std::ostream &os) const
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
a collection of properties mapping from name to the property itself
Definition: property.h:76
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
bool computeMarginals(SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices)
void sigquit_handler(int sig)
Definition: g2o.cpp:90
std::set< int > dimensions() const
std::vector< G2OBatchStatistics > BatchStatisticsContainer
Definition: batch_stats.h:81
Sparse matrix which uses blocks.
OptimizationAlgorithm * construct(const std::string &tag, OptimizationAlgorithmProperty &solverProperty) const
bool parsedParam(const std::string &paramFlag) const
void sigquit_handler ( int  sig)

Definition at line 90 of file g2o.cpp.

References __PRETTY_FUNCTION__, and hasToStop.

Referenced by main().

91 {
92  if (sig == SIGINT) {
93  hasToStop = 1;
94  static int cnt = 0;
95  if (cnt++ == 2) {
96  cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
97  exit(1);
98  }
99  }
100 }
#define __PRETTY_FUNCTION__
Definition: macros.h:89
static bool hasToStop
Definition: g2o.cpp:60

Variable Documentation

bool hasToStop =false
static

Definition at line 60 of file g2o.cpp.

Referenced by main(), and sigquit_handler().