g2o
Loading...
Searching...
No Matches
Functions | Variables
g2o_hierarchical.cpp File Reference
#include <signal.h>
#include <algorithm>
#include <cassert>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include "edge_creator.h"
#include "edge_labeler.h"
#include "edge_types_cost_function.h"
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/g2o_common.h"
#include "g2o/apps/g2o_cli/output_helper.h"
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/factory.h"
#include "g2o/core/hyper_dijkstra.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/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 "star.h"
#include "g2o/types/slam3d/parameter_camera.h"
#include "g2o/types/slam3d/parameter_se3_offset.h"
#include "simple_star_ops.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

◆ main()

int main ( int  argc,
char **  argv 
)

Definition at line 96 of file g2o_hierarchical.cpp.

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

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::RobustKernelFactory::creator(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::dimensions(), g2o::HyperGraph::edges(), g2o::RobustKernelFactory::fillKnownKernels(), g2o::SparseOptimizer::findGauge(), g2o::Star::gauge(), g2o::SparseOptimizer::gaugeFreedom(), hasToStop, g2o::HyperGraph::Vertex::id(), g2o::Parameter::id(), g2o::SparseOptimizer::initializeOptimization(), g2o::Factory::instance(), g2o::OptimizationAlgorithmFactory::instance(), g2o::RobustKernelFactory::instance(), 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::Factory::printRegisteredTypes(), 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::Factory::tag(), g2o::HyperGraph::Edge::vertices(), g2o::HyperGraph::vertices(), g2o::HyperDijkstra::visited(), and g2o::PropertyMap::writeToCSV().

◆ sigquit_handler()

void sigquit_handler ( int  sig)

Definition at line 70 of file g2o_hierarchical.cpp.

70 {
71 if (sig == SIGINT) {
72 hasToStop = 1;
73 static int cnt = 0;
74 if (cnt++ == 2) {
75 cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
76 exit(1);
77 }
78 }
79}
#define __PRETTY_FUNCTION__
Definition macros.h:90

References __PRETTY_FUNCTION__, and hasToStop.

Referenced by main().

Variable Documentation

◆ hasToStop

bool hasToStop = false
static

Definition at line 65 of file g2o_hierarchical.cpp.

Referenced by main(), and sigquit_handler().