96 {
97 int starIterations;
98 int highIterations;
99 int lowIterations;
100 bool verbose;
101
102 string inputFilename;
103 string gnudump;
104 string outputfilename;
105
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
120 int hierarchicalDiameter;
121 int updateGraphEachN = 10;
122 string summaryFile;
123 string dummy;
124
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
139
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");
174 "graph file which will be processed", true);
175
177
178
179
180
181
184
185
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
206 if (robustKernel.size() > 0) {
208 }
209
213
214
215 if (loadLookup.size() > 0) {
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());
248 cerr << "done." << endl;
249 return 0;
250 }
251
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
259 if (p0) {
261 if (originalParams) {
262 cerr << "ORIGINAL PARAMS" << endl;
264 se3OffsetParam->
setId(100);
266 std::vector<int> depthCamHParamsIds(1);
267 depthCamHParamsIds[0] = se3OffsetParam->
id();
269 "EDGE_SE3_TRACKXYZ", depthCamHParamsIds);
270 }
271 }
272
274
275 if (optimizer.
vertices().size() == 0) {
276 cerr << "Graph contains no vertices" << endl;
277 return 1;
278 }
279
280
283 solverFactory->
construct(strSolver, solverProperty);
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();
299 cerr << "The selected solver is not suitable for optimizing the given graph"
300 << endl;
301 return 3;
302 }
304 cerr << "The selected solver is not suitable for optimizing the given graph"
305 << endl;
306 return 3;
307 }
308
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
328
329
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
344
345
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;
356 }
357 } else {
358 cerr << "# graph is fixed by priors" << endl;
359 }
360
361
364 d.shortestPaths(gauge, &f);
365
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
375
377
380
381 cerr << "Initial chi2 = " << FIXED(loadChi) << endl;
382
385
388
389
390
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
400
402
404
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;
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) {
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();
434 eset.insert(e);
435 for (
size_t i = 0; i < e->
vertices().size(); i++) {
437 }
438 }
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");
450 starStream.close();
451
452 ofstream hstarStream("hstars.g2o");
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) {
463 }
468
471
473
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);
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) {
496 }
500 cerr << "done" << endl;
501 if (debug) {
502 ofstream os("debug_low_level.g2o");
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) {
513 }
514 for (HyperGraph::VertexSet::iterator it = hgauge.begin(); it != hgauge.end();
515 ++it) {
518 }
523
524 cerr << "relaxing the full problem" << endl;
525 for (OptimizableGraph::VertexSet::iterator it = vset.begin();
526 it != vset.end(); ++it) {
529 }
530 for (HyperGraph::VertexSet::iterator it = hgauge.begin(); it != hgauge.end();
531 ++it) {
534 }
537 int result = optimizer.
optimize(lowIterations);
538 if (result < 0) cerr << "failure in low level optimization" << endl;
539
542
543 if (summaryFile != "") {
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) {
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) {
567 if (e->
level() == 0) {
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
587
594
595 ofstream os;
596 os.open(summaryFile.c_str(), ios::app);
598 }
599
600 if (outputfilename.size() > 0) {
601 if (outputfilename == "-") {
602 cerr << "saving to stdout";
604 } else {
605 cerr << "saving " << outputfilename << " ... ";
606 ofstream os(outputfilename.c_str());
608 }
609 cerr << "done." << endl;
610 }
611
612
613
614
615
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.
void printRegisteredTypes(std::ostream &os, bool comment=false) const
static Factory * instance()
return the instance
const std::string & tag(const HyperGraph::HyperGraphElement *v) const
return the TAG given a vertex
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.
a collection of properties mapping from name to the property itself
P * makeProperty(const std::string &name_, const typename P::ValueType &v)
void writeToCSV(std::ostream &os) const
void fillKnownKernels(std::vector< std::string > &types) const
AbstractRobustKernelCreator * creator(const std::string &tag) const
static RobustKernelFactory * instance()
return the instance
void computeActiveErrors()
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.
double activeChi2() const
void sigquit_handler(int sig)
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
void loadStandardTypes(DlWrapper &dlTypesWrapper, int argc, char **argv)
std::set< Star * > StarSet
void loadStandardSolver(DlWrapper &dlSolverWrapper, int argc, char **argv)
std::map< HyperGraph::Edge *, Star * > EdgeStarMap
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)
describe the properties of a solver
HyperGraph::EdgeSet & starFrontierEdges()
edges in the high level that lead to some node owned by a different star
HyperGraph::EdgeSet _starEdges
edges in the star
HyperGraph::VertexSet & gauge()
set of nodes to keep fixed in the optimization