g2o
Functions | Variables
g2o_hierarchical.cpp File Reference
#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/output_helper.h"
#include "g2o/apps/g2o_cli/g2o_common.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/robust_kernel.h"
#include "g2o/core/robust_kernel_factory.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 "edge_labeler.h"
#include "edge_creator.h"
#include "edge_types_cost_function.h"
#include "star.h"
#include "simple_star_ops.h"
#include "g2o/types/slam3d/parameter_camera.h"
#include "g2o/types/slam3d/parameter_se3_offset.h"
Include dependency graph for g2o_hierarchical.cpp:

Go to the source code of this file.

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 91 of file g2o_hierarchical.cpp.

References g2o::Star::_starEdges, g2o::SparseOptimizer::activeChi2(), g2o::EdgeCreator::addAssociation(), g2o::OptimizableGraph::addParameter(), CL_RED, g2o::SparseOptimizer::computeActiveErrors(), g2o::computeBorder(), g2o::SparseOptimizer::computeInitialGuess(), g2o::computeSimpleStars(), g2o::AbstractRobustKernelCreator::construct(), g2o::OptimizationAlgorithmFactory::construct(), g2o::constructEdgeStarMap(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::dimensions(), g2o::HyperGraph::edges(), g2o::SparseOptimizer::findGauge(), g2o::Star::gauge(), g2o::SparseOptimizer::gaugeFreedom(), hasToStop, g2o::Parameter::id(), g2o::HyperGraph::Vertex::id(), g2o::SparseOptimizer::initializeOptimization(), g2o::OptimizableGraph::isSolverSuitable(), g2o::OptimizableGraph::Edge::level(), g2o::OptimizationAlgorithmFactory::listSolvers(), g2o::OptimizableGraph::load(), g2o::loadStandardSolver(), g2o::loadStandardTypes(), g2o::PropertyMap::makeProperty(), g2o::SparseOptimizer::optimize(), g2o::CommandArgs::param(), g2o::OptimizableGraph::parameter(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::OptimizableGraph::save(), g2o::OptimizableGraph::saveSubset(), g2o::SparseOptimizer::setAlgorithm(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::SparseOptimizer::setForceStopFlag(), g2o::Parameter::setId(), g2o::OptimizableGraph::setRenamedTypesFromString(), g2o::SparseOptimizer::setVerbose(), g2o::HyperDijkstra::shortestPaths(), sigquit_handler(), g2o::Star::starFrontierEdges(), g2o::HyperGraph::Edge::vertices(), g2o::HyperGraph::vertices(), g2o::HyperDijkstra::visited(), and g2o::PropertyMap::writeToCSV().

92 {
93 
94  int starIterations;
95  int highIterations;
96  int lowIterations;
97  bool verbose;
98  //bool useNewTypes;
99  string inputFilename;
100  string gnudump;
101  string outputfilename;
102  //string strMethod;
103  string strSolver;
104  string strHSolver;
105  string loadLookup;
106  bool initialGuess;
107  bool listTypes;
108  bool listSolvers;
109  bool listRobustKernels;
110  bool guiOut;
111  bool computeMarginals;
112  double huberWidth;
113  bool debug;
114  double uThreshold;
115  string robustKernel;
116  //double lambdaInit;
117  int hierarchicalDiameter;
118  int updateGraphEachN = 10;
119  string summaryFile;
120  string dummy;
121  // command line parsing
122  CommandArgs arg;
123  arg.param("si", starIterations, 30, "perform n iterations to build the stars");
124  arg.param("hi", highIterations, 100, "perform n iterations to construct the hierarchy");
125  arg.param("li", lowIterations, 100, "perform n iterations on the low level");
126  arg.param("v", verbose, false, "verbose output of the optimization process");
127  arg.param("uThreshold", uThreshold, -1., "rejection threshold for underdetermined vertices");
128  arg.param("hierarchicalDiameter", hierarchicalDiameter, -1 , "selects the diameter of the stars in the hierarchical graph");
129  arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
130  //arg.param("useNewTypes", useNewTypes, false, "if true remaps the slam3d old types into the new ones");
131  arg.param("debug", debug, false, "print shit load of things for debugging");
132  arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes, (default: 10)");
133  arg.param("guiout", guiOut, false, "gui output while running incrementally");
134  arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
135  arg.param("robustKernel", robustKernel, "", "use this robust error function");
136  arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)");
137  arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY");
138  arg.param("huberWidth", huberWidth, -1., "width for the robust Huber Kernel (only if robustKernel)");
139  arg.param("o", outputfilename, "", "output final version of the graph");
140  arg.param("solver", strSolver, "lm_var_cholmod", "specify which solver to use underneat");
141  arg.param("hsolver", strHSolver, "gn_var_cholmod", "specify which solver to use for the high level");
142  arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
143  arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
144  arg.param("listTypes", listTypes, false, "list the registered types");
145  arg.param("listSolvers", listSolvers, false, "list the available solvers");
146  arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels");
147 
148  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");
149  arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument");
150  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
151 
152  arg.parseArgs(argc, argv);
153 
154  // if (useNewTypes){
155  // loadLookup=newTypesMapping+loadLookup;
156  // }
157  // registering all the types from the libraries
158  DlWrapper dlTypesWrapper;
159  loadStandardTypes(dlTypesWrapper, argc, argv);
160 
161  // register all the solvers
162  OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
163  DlWrapper dlSolverWrapper;
164  loadStandardSolver(dlSolverWrapper, argc, argv);
165  if (listSolvers)
166  solverFactory->listSolvers(cerr);
167 
168  if (listTypes) {
169  Factory::instance()->printRegisteredTypes(cout, true);
170  }
171 
172  if (listRobustKernels) {
173  std::vector<std::string> kernels;
174  RobustKernelFactory::instance()->fillKnownKernels(kernels);
175  cout << "Robust Kernels:" << endl;
176  for (size_t i = 0; i < kernels.size(); ++i) {
177  cout << kernels[i] << endl;
178  }
179  }
180 
181  AbstractRobustKernelCreator* kernelCreator=0;
182  if (robustKernel.size() > 0) {
183  kernelCreator = RobustKernelFactory::instance()->creator(robustKernel);
184  }
185 
186  SparseOptimizer optimizer;
187  optimizer.setVerbose(verbose);
188  optimizer.setForceStopFlag(&hasToStop);
189 
190  // Loading the input data
191  if (loadLookup.size() > 0) {
192  optimizer.setRenamedTypesFromString(loadLookup);
193  }
194  if (inputFilename.size() == 0) {
195  cerr << "No input data specified" << endl;
196  return 0;
197  } else if (inputFilename == "-") {
198  cerr << "Read input from stdin" << endl;
199  if (!optimizer.load(cin)) {
200  cerr << "Error loading graph" << endl;
201  return 2;
202  }
203  } else {
204  cerr << "Read input from " << inputFilename << endl;
205  ifstream ifs(inputFilename.c_str());
206  if (!ifs) {
207  cerr << "Failed to open file" << endl;
208  return 1;
209  }
210  if (!optimizer.load(ifs)) {
211  cerr << "Error loading graph" << endl;
212  return 2;
213  }
214  }
215  cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
216  cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
217 
218 
219  OptimizableGraph::EdgeSet originalEdges=optimizer.edges();
220 
221  if (0 && outputfilename.size() > 0) {
222  cerr << "saving " << outputfilename << " ... ";
223  ofstream os(outputfilename.c_str());
224  optimizer.save(os);
225  cerr << "done." << endl;
226  return 0;
227  }
228 
229  EdgeCreator creator;
230  creator.addAssociation("VERTEX_SE2;VERTEX_SE2;","EDGE_SE2");
231  creator.addAssociation("VERTEX_SE2;VERTEX_XY;","EDGE_SE2_XY");
232  creator.addAssociation("VERTEX_SE3:QUAT;VERTEX_SE3:QUAT;","EDGE_SE3:QUAT");
233  creator.addAssociation("VERTEX_SE3_NEW;VERTEX_SE3_NEW;","EDGE_SE3_NEW");
234 
235  Parameter* p0 = optimizer.parameter(0);
236  if (p0){
237  ParameterSE3Offset* originalParams = dynamic_cast<ParameterSE3Offset*>(p0);
238  if (originalParams) {
239  cerr << "ORIGINAL PARAMS" << endl;
240  ParameterSE3Offset* se3OffsetParam = new ParameterSE3Offset();
241  se3OffsetParam->setId(100);
242  optimizer.addParameter(se3OffsetParam);
243  std::vector<int> depthCamHParamsIds(1);
244  depthCamHParamsIds[0]=se3OffsetParam->id();
245  creator.addAssociation("VERTEX_SE3:QUAT;VERTEX_TRACKXYZ;","EDGE_SE3_TRACKXYZ", depthCamHParamsIds);
246  }
247  }
248 
249  EdgeLabeler labeler(&optimizer);
250 
251  if (optimizer.vertices().size() == 0) {
252  cerr << "Graph contains no vertices" << endl;
253  return 1;
254  }
255 
256  // allocating the desired solver + testing whether the solver is okay
257  OptimizationAlgorithmProperty solverProperty, hsolverProperty;
258  OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty);
259  OptimizationAlgorithm* hsolver = solverFactory->construct(strHSolver, hsolverProperty);
260  if (! solver) {
261  cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
262  return 0;
263  }
264  if (! hsolver) {
265  cerr << "Error allocating hsolver. Allocating \"" << strHSolver << "\" failed!" << endl;
266  return 0;
267  }
268 
269  set<int> vertexDimensions = optimizer.dimensions();
270  if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
271  cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
272  return 3;
273  }
274  if (! optimizer.isSolverSuitable(hsolverProperty, vertexDimensions)) {
275  cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
276  return 3;
277  }
278 
279  optimizer.setAlgorithm(solver);
280 
281  int poseDim=*vertexDimensions.rbegin();
282  string backboneVertexType;
283  string backboneEdgeType;
284  switch(poseDim){
285  case 3:
286  if (hierarchicalDiameter == -1)
287  hierarchicalDiameter = 30;
288  backboneEdgeType = "EDGE_SE2";
289  backboneVertexType = "VERTEX_SE2";
290  if (uThreshold <0){
291  uThreshold =1e-5;
292  }
293  break;
294  case 6:
295  if (hierarchicalDiameter == -1)
296  hierarchicalDiameter = 4;
297  backboneEdgeType = "EDGE_SE3:QUAT";
298  backboneVertexType = "VERTEX_SE3:QUAT";
299  // if (useNewTypes){
300  // backboneEdgeType = "EDGE_SE3_NEW";
301  // backboneVertexType = "VERTEX_SE3_NEW";
302  // }
303 
304  if (uThreshold <0){
305  uThreshold =1e-3;
306  }
307  break;
308  default:
309  cerr << "Fatal: unknown backbone type. The largest vertex dimension is: " << poseDim << "." << endl
310  <<"Exiting." << endl;
311  return -1;
312  }
313 
314  // here we need to chop the graph into many lil pieces
315 
316 
317  // check for vertices to fix to remove DoF
318  bool gaugeFreedom = optimizer.gaugeFreedom();
319  OptimizableGraph::Vertex* gauge=optimizer.findGauge();
320 
321 
322 
323 
324  if (gaugeFreedom) {
325  if (! gauge) {
326  cerr << "# cannot find a vertex to fix in this thing" << endl;
327  return 2;
328  } else {
329  cerr << "# graph is fixed by node " << gauge->id() << endl;
330  gauge->setFixed(true);
331  }
332  } else {
333  cerr << "# graph is fixed by priors" << endl;
334  }
335 
336 
337 
338 
339 
340 
341  // sanity check
342  HyperDijkstra d(&optimizer);
344  d.shortestPaths(gauge,&f);
345  //cerr << PVAR(d.visited().size()) << endl;
346 
347  if (d.visited().size()!=optimizer.vertices().size()) {
348  cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
349  cerr << "visited: " << d.visited().size() << endl;
350  cerr << "vertices: " << optimizer.vertices().size() << endl;
351  }
352 
353 
354  // BATCH optimization
355 
356  optimizer.initializeOptimization();
357 
358  optimizer.computeActiveErrors();
359  double loadChi=optimizer.activeChi2();
360 
361  cerr << "Initial chi2 = " << FIXED(loadChi) << endl;
362 
363 
364  if (initialGuess)
365  optimizer.computeInitialGuess();
366  signal(SIGINT, sigquit_handler);
367 
368  optimizer.computeActiveErrors();
369  double initChi = optimizer.activeChi2();
370 
371  // if (robustKernel) {
372  //cerr << "# Preparing robust error function ... ";
373  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
374  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
375  if (kernelCreator) {
376  e->setRobustKernel(kernelCreator->construct());
377  if (huberWidth > 0)
378  e->robustKernel()->setDelta(huberWidth);
379  }
380  }
381  //cerr << "done." << endl;
382  //}
383  optimizer.computeActiveErrors();
384 
385 
386  StarSet stars;
387 
388 
389  computeSimpleStars(stars, &optimizer, &labeler, &creator,
390  gauge, backboneEdgeType, backboneVertexType, 0, hierarchicalDiameter,
391  1, starIterations, uThreshold, debug);
392 
393  cerr << "stars computed, stars.size()= " << stars.size() << endl;
394 
395  cerr << "hierarchy done, determining border" << endl;
396  EdgeStarMap hesmap;
397  constructEdgeStarMap(hesmap, stars, false);
398  computeBorder(stars, hesmap);
399 
400  OptimizableGraph::EdgeSet eset;
401  OptimizableGraph::VertexSet vset;
402  OptimizableGraph::EdgeSet heset;
403  OptimizableGraph::VertexSet hvset;
404  HyperGraph::VertexSet hgauge;
405  for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++) {
406 
407  Star* s=*it;
408  if (hgauge.empty())
409  hgauge=s->gauge();
410 
411  for (HyperGraph::VertexSet::iterator git=s->gauge().begin();
412  git != s->gauge().end(); git++) {
413  hvset.insert(*git);
414  }
415 
416  for (HyperGraph::EdgeSet::iterator iit=s->_starEdges.begin(); iit!=s->_starEdges.end(); iit++){
418  eset.insert(e);
419  for (size_t i=0; i<e->vertices().size(); i++){
420  vset.insert(e->vertices()[i]);
421  }
422  }
423  for (HyperGraph::EdgeSet::iterator iit=s->starFrontierEdges().begin(); iit!=s->starFrontierEdges().end(); iit++){
425  heset.insert(e);
426  }
427  }
428  cerr << "eset.size()= " << eset.size() << endl;
429  cerr << "heset.size()= " << heset.size() << endl;
430 
431  ofstream starStream("stars.g2o");
432  optimizer.saveSubset(starStream, eset);
433  starStream.close();
434 
435  ofstream hstarStream("hstars.g2o");
436  optimizer.saveSubset(hstarStream, heset);
437  hstarStream.close();
438 
439  cerr << "stars done!" << endl;
440 
441  cerr << "optimizing the high layer" << endl;
442  for (HyperGraph::VertexSet::iterator it = hgauge.begin(); it!=hgauge.end(); it++){
443  OptimizableGraph::Vertex* g=dynamic_cast<OptimizableGraph::Vertex*>(*it);
444  g->setFixed(true);
445  }
446  optimizer.setAlgorithm(hsolver);
447  optimizer.initializeOptimization(heset);
448  optimizer.setVerbose(true);
449  if (initialGuess)
450  optimizer.computeInitialGuess();
451 
452  optimizer.computeActiveErrors();
453  double hInitChi = optimizer.activeChi2();
454 
455  optimizer.optimize(highIterations);
456 
457  optimizer.computeActiveErrors();
458  double hFinalChi = optimizer.activeChi2();
459 
460  cerr << "done" << endl;
461 
462 
463  if (! kernelCreator) {
464  cerr << "# Robust error function disabled ";
465  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
466  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
467  e->setRobustKernel(0);
468  }
469  cerr << "done." << endl;
470  } else {
471  cerr << "# Preparing robust error function ay low level done";
472  }
473 
474  cerr << "fixing the hstructure, and optimizing the floating nodes" << endl;
475  for (OptimizableGraph::VertexSet::iterator it = hvset.begin(); it!=hvset.end(); it++){
476  OptimizableGraph::Vertex* g=dynamic_cast<OptimizableGraph::Vertex*>(*it);
477  g->setFixed(true);
478  }
479  optimizer.initializeOptimization(eset);
480  optimizer.computeInitialGuess();
481  optimizer.optimize(1);
482  cerr << "done" << endl;
483  if (debug) {
484  ofstream os("debug_low_level.g2o");
485  optimizer.saveSubset(os,eset);
486  }
487 
488 
489  cerr << "adding the original constraints, locking hierarchical solution and optimizing the free variables" << endl;
490  for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it!=vset.end(); it++){
491  OptimizableGraph::Vertex* g=dynamic_cast<OptimizableGraph::Vertex*>(*it);
492  g->setFixed(true);
493  }
494  for (HyperGraph::VertexSet::iterator it = hgauge.begin(); it!=hgauge.end(); it++){
495  OptimizableGraph::Vertex* g=dynamic_cast<OptimizableGraph::Vertex*>(*it);
496  g->setFixed(true);
497  }
498  optimizer.setAlgorithm(solver);
499  optimizer.initializeOptimization(0);
500  optimizer.computeInitialGuess();
501  optimizer.optimize(lowIterations);
502 
503 
504  cerr << "relaxing the full problem" << endl;
505  for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it!=vset.end(); it++){
506  OptimizableGraph::Vertex* g=dynamic_cast<OptimizableGraph::Vertex*>(*it);
507  g->setFixed(false);
508  }
509  for (HyperGraph::VertexSet::iterator it = hgauge.begin(); it!=hgauge.end(); it++){
510  OptimizableGraph::Vertex* g=dynamic_cast<OptimizableGraph::Vertex*>(*it);
511  g->setFixed(true);
512  }
513  optimizer.setAlgorithm(solver);
514  optimizer.initializeOptimization(0);
515  int result = optimizer.optimize(lowIterations);
516  if (result <0)
517  cerr << "failure in low level optimization" << endl;
518 
519  optimizer.computeActiveErrors();
520  double finalChi=optimizer.activeChi2();
521 
522  if (summaryFile!="") {
523  PropertyMap summary;
524  summary.makeProperty<StringProperty>("filename", inputFilename);
525  summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size());
526 
527  int nLandmarks=0;
528  int nPoses=0;
529  int maxDim = *vertexDimensions.rbegin();
530  for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
531  OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
532  if (v->dimension() != maxDim) {
533  nLandmarks++;
534  } else
535  nPoses++;
536  }
537 
538  int nEdges=0;
539  set<string> edgeTypes;
540  for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){
541  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*> (*it);
542  if (e->level()==0) {
543  edgeTypes.insert(Factory::instance()->tag(e));
544  nEdges++;
545  }
546  }
547  stringstream edgeTypesString;
548  for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){
549  edgeTypesString << *it << " ";
550  }
551 
552  summary.makeProperty<IntProperty>("n_edges", nEdges);
553  summary.makeProperty<IntProperty>("n_poses", nPoses);
554  summary.makeProperty<IntProperty>("n_landmarks", nLandmarks);
555  summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str());
556  summary.makeProperty<DoubleProperty>("load_chi", loadChi);
557  summary.makeProperty<DoubleProperty>("init_chi", initChi);
558  summary.makeProperty<DoubleProperty>("final_chi", finalChi);
559  summary.makeProperty<StringProperty>("solver", strSolver);
560  summary.makeProperty<StringProperty>("robustKernel", robustKernel);
561 
562  summary.makeProperty<IntProperty>("n_stars", stars.size());
563  summary.makeProperty<IntProperty>("n_star_edges", eset.size());
564  summary.makeProperty<IntProperty>("n_star_h_edges", heset.size());
565  summary.makeProperty<IntProperty>("n_star_h_vertices", hvset.size());
566  summary.makeProperty<DoubleProperty>("h_initChi", hInitChi);
567  summary.makeProperty<DoubleProperty>("h_finalChi", hFinalChi);
568 
569 
570  ofstream os;
571  os.open(summaryFile.c_str(), ios::app);
572  summary.writeToCSV(os);
573  }
574 
575 
576  if (outputfilename.size() > 0) {
577  if (outputfilename == "-") {
578  cerr << "saving to stdout";
579  optimizer.saveSubset(cout,originalEdges);
580  } else {
581  cerr << "saving " << outputfilename << " ... ";
582  ofstream os(outputfilename.c_str());
583  optimizer.saveSubset(os,originalEdges);
584  }
585  cerr << "done." << endl;
586  }
587 
588  // destroy all the singletons
589  //Factory::destroy();
590  //OptimizationAlgorithmFactory::destroy();
591  //HyperGraphActionLibrary::destroy();
592 
593  return 0;
594 }
virtual void computeInitialGuess()
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 loadStandardTypes(DlWrapper &dlTypesWrapper, int argc, char **argv)
Definition: g2o_common.cpp:96
void constructEdgeStarMap(EdgeStarMap &esmap, StarSet &stars, bool low)
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:136
HyperGraph::EdgeSet & starFrontierEdges()
edges in the high level that lead to some node owned by a different star
Definition: star.h:47
int optimize(int iterations, bool online=false)
#define CL_RED(s)
Definition: color_macros.h:46
int level() const
returns the level of the edge
void setRobustKernel(RobustKernel *ptr)
create solvers based on their short name
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
void setId(int id_)
Definition: parameter.cpp:35
void computeBorder(StarSet &stars, EdgeStarMap &hesmap)
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
P * makeProperty(const std::string &name_, const typename P::ValueType &v)
Definition: property.h:119
HyperGraph::EdgeSet _starEdges
edges in the star
Definition: star.h:60
const VertexContainer & vertices() const
Definition: hyper_graph.h:178
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)
Definition: star.h:26
const EdgeSet & edges() const
Definition: hyper_graph.h:230
bool saveSubset(std::ostream &os, HyperGraph::VertexSet &vset, int level=0)
save a subgraph to a stream. Again uses the Factory system.
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)
int id() const
Definition: parameter.h:45
void sigquit_handler(int sig)
void param(const std::string &name, bool &p, bool defValue, const std::string &desc)
std::set< Star * > StarSet
Definition: star.h:71
void writeToCSV(std::ostream &os) const
Definition: property.cpp:74
bool addParameter(Parameter *p)
std::map< HyperGraph::Edge *, Star * > EdgeStarMap
Definition: star.h:72
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...
Parameter * parameter(int id)
void computeSimpleStars(StarSet &stars, SparseOptimizer *optimizer, EdgeLabeler *labeler, EdgeCreator *creator, OptimizableGraph::Vertex *gauge_, std::string edgeTag, std::string vertexTag, int level, int step, int backboneIterations, int starIterations, double rejectionThreshold, bool debug)
HyperGraph::VertexSet & gauge()
set of nodes to keep fixed in the optimization
Definition: star.h:49
virtual RobustKernel * construct()=0
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
static bool hasToStop
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
a collection of properties mapping from name to the property itself
Definition: property.h:76
Generic interface for a non-linear solver operating on a graph.
std::set< int > dimensions() const
OptimizationAlgorithm * construct(const std::string &tag, OptimizationAlgorithmProperty &solverProperty) const
double activeChi2() const
bool addAssociation(const std::string &vertexTypes, const std::string &edgeType)
void sigquit_handler ( int  sig)

Definition at line 64 of file g2o_hierarchical.cpp.

References __PRETTY_FUNCTION__, and hasToStop.

Referenced by main().

65 {
66  if (sig == SIGINT) {
67  hasToStop = 1;
68  static int cnt = 0;
69  if (cnt++ == 2) {
70  cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
71  exit(1);
72  }
73  }
74 }
#define __PRETTY_FUNCTION__
Definition: macros.h:89
static bool hasToStop

Variable Documentation

bool hasToStop =false
static

Definition at line 59 of file g2o_hierarchical.cpp.

Referenced by main(), and sigquit_handler().