optimizable_graph.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #include "optimizable_graph.h"
28 
29 #include <cassert>
30 #include <iostream>
31 #include <iomanip>
32 #include <fstream>
33 #include <algorithm>
34 
35 #include <Eigen/Dense>
36 
37 #include "estimate_propagator.h"
38 #include "factory.h"
40 #include "hyper_graph_action.h"
41 #include "cache.h"
42 #include "robust_kernel.h"
43 
44 #include "../stuff/macros.h"
45 #include "../stuff/color_macros.h"
46 #include "../stuff/string_tools.h"
47 #include "../stuff/misc.h"
48 
49 namespace g2o {
50 
51  using namespace std;
52 
54  _next = 0;
55  }
56 
58  if (_next)
59  delete _next;
60  }
61 
62 
64  HyperGraph::Vertex(),
65  _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false),
66  _colInHessian(-1), _cacheContainer(0)
67  {
68  }
69 
71  if (! _cacheContainer)
72  _cacheContainer = new CacheContainer(this);
73  return _cacheContainer;
74  }
75 
76 
78  if (_cacheContainer){
81  }
82  }
83 
85  {
86  if (_cacheContainer)
87  delete (_cacheContainer);
88  if (_userData)
89  delete _userData;
90  }
91 
93  {
94  return 0;
95  }
96 
98  {
99  bool ret = setEstimateDataImpl(v);
100  updateCache();
101  return ret;
102  }
103 
105  {
106  return false;
107  }
108 
110  {
111  return -1;
112  }
113 
115  {
116  bool ret = setMinimalEstimateDataImpl(v);
117  updateCache();
118  return ret;
119  }
120 
122  {
123  return false;
124  }
125 
127  {
128  return -1;
129  }
130 
131 
133  HyperGraph::Edge(),
134  _dimension(-1), _level(0), _robustKernel(0)
135  {
136  }
137 
139  {
140  delete _robustKernel;
141  }
142 
144  if (! _vertices.size())
145  return 0;
147  if (!v)
148  return 0;
149  return v->graph();
150  }
151 
153  if (! _vertices.size())
154  return 0;
156  if (!v)
157  return 0;
158  return v->graph();
159  }
160 
161  bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){
162  if ((int)_parameters.size()<=argNum)
163  return false;
164  if (argNum<0)
165  return false;
166  *_parameters[argNum] = 0;
167  _parameterIds[argNum] = paramId;
168  return true;
169  }
170 
172  if (!graph()) {
173  cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl;
174  return false;
175  }
176 
177  assert (_parameters.size() == _parameterIds.size());
178  //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl;
179  for (size_t i=0; i<_parameters.size(); i++){
180  int index = _parameterIds[i];
181  *_parameters[i] = graph()->parameter(index);
182  if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){
183  cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl;
184  }
185  if (!*_parameters[i]) {
186  cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl;
187  return false;
188  }
189  }
190  return true;
191  }
192 
194  {
195  if (_robustKernel)
196  delete _robustKernel;
197  _robustKernel = ptr;
198  }
199 
201  return true;
202  }
203 
205  {
206  return false;
207  }
208 
210  {
211  return false;
212  }
213 
215  {
216  return -1;
217  }
218 
220  return false;
221  }
222 
223 
225  {
226  // TODO
227  return 0;
228  }
229 
230 
232  {
233  _nextEdgeId = 0; _edge_has_id = false;
235  }
236 
238  {
239  clear();
240  clearParameters();
241  }
242 
244  {
245  Vertex* inserted = vertex(v->id());
246  if (inserted) {
247  cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl;
248  assert(0 && "Vertex with this ID already contained in the graph");
249  return false;
250  }
252  assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex");
253  if (ov->_graph != 0 && ov->_graph != this) {
254  cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl;
255  assert(0 && "Vertex already registered with another graph");
256  return false;
257  }
258  if (userData)
259  ov->setUserData(userData);
260  ov->_graph=this;
261  return HyperGraph::addVertex(v);
262  }
263 
265  {
266  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_);
267  assert(e && "Edge does not inherit from OptimizableGraph::Edge");
268  if (! e)
269  return false;
270  bool eresult = HyperGraph::addEdge(e);
271  if (! eresult)
272  return false;
273  e->_internalId = _nextEdgeId++;
274  if (! e->resolveParameters()){
275  cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl;
276  return false;
277  }
278  if (! e->resolveCaches()){
279  cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl;
280  return false;
281  }
283 
284  return true;
285  }
286 
287  int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;}
288 
290 {
291  double chi = 0.0;
292  for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) {
293  const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
294  chi += e->chi2();
295  }
296  return chi;
297 }
298 
300 {
301  for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
302  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
303  v->push();
304  }
305 }
306 
308 {
309  for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
310  OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
311  v->pop();
312  }
313 }
314 
316 {
317  for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
318  OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
319  v->discardTop();
320  }
321 }
322 
324 {
325  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
326  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
327  v->push();
328  }
329 }
330 
332 {
333  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
334  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
335  v->pop();
336  }
337 }
338 
340 {
341  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
342  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
343  v->discardTop();
344  }
345 }
346 
348 {
349  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
350  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
351  v->setFixed(fixed);
352  }
353 }
354 
355 
356 bool OptimizableGraph::load(istream& is, bool createEdges)
357 {
358  // scna for the paramers in the whole file
359  if (!_parameters.read(is,&_renamedTypesLookup))
360  return false;
361 #ifndef NDEBUG
362  cerr << "Loaded " << _parameters.size() << " parameters" << endl;
363 #endif
364  is.clear();
365  is.seekg(ios_base::beg);
366  set<string> warnedUnknownTypes;
367  stringstream currentLine;
368  string token;
369 
370  Factory* factory = Factory::instance();
371  HyperGraph::GraphElemBitset elemBitset;
372  elemBitset[HyperGraph::HGET_PARAMETER] = 1;
373  elemBitset.flip();
374 
375  Vertex* previousVertex = 0;
376  Data* previousData = 0;
377 
378  while (1) {
379  int bytesRead = readLine(is, currentLine);
380  if (bytesRead == -1)
381  break;
382  currentLine >> token;
383  //cerr << "Token=" << token << endl;
384  if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
385  continue;
386 
387  // handle commands encoded in the file
388  bool handledCommand = false;
389 
390  if (token == "FIX") {
391  handledCommand = true;
392  int id;
393  while (currentLine >> id) {
395  if (v) {
396 # ifndef NDEBUG
397  cerr << "Fixing vertex " << v->id() << endl;
398 # endif
399  v->setFixed(true);
400  } else {
401  cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl;
402  }
403  }
404  }
405 
406  if (handledCommand)
407  continue;
408 
409  // do the mapping to an internal type if it matches
410  if (_renamedTypesLookup.size() > 0) {
411  map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token);
412  if (foundIt != _renamedTypesLookup.end()) {
413  token = foundIt->second;
414  }
415  }
416 
417  if (! factory->knowsTag(token)) {
418  if (warnedUnknownTypes.count(token) != 1) {
419  warnedUnknownTypes.insert(token);
420  cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl;
421  }
422  continue;
423  }
424 
425  HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
426  if (dynamic_cast<Vertex*>(element)) { // it's a vertex type
427  //cerr << "it is a vertex" << endl;
428  previousData = 0;
429  Vertex* v = static_cast<Vertex*>(element);
430  int id;
431  currentLine >> id;
432  bool r = v->read(currentLine);
433  if (! r)
434  cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl;
435  v->setId(id);
436  if (!addVertex(v)) {
437  cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl;
438  delete v;
439  } else {
440  previousVertex = v;
441  }
442  }
443  else if (dynamic_cast<Edge*>(element)) {
444  //cerr << "it is an edge" << endl;
445  previousData = 0;
446  Edge* e = static_cast<Edge*>(element);
447  int numV = e->vertices().size();
448  if (_edge_has_id){
449  int id;
450  currentLine >> id;
451  e->setId(id);
452  }
453  //cerr << PVAR(token) << " " << PVAR(numV) << endl;
454  if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way
455  int id1, id2;
456  currentLine >> id1 >> id2;
457  Vertex* from = vertex(id1);
458  Vertex* to = vertex(id2);
459  int doInit=0;
460  if ((!from || !to) ) {
461  if (! createEdges) {
462  cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token << " " << id1 << " <-> " << id2 << endl;
463  delete e;
464  } else {
465  if (! from) {
466  from=e->createFrom();
467  from->setId(id1);
468  addVertex(from);
469  doInit=2;
470  }
471  if (! to) {
472  to=e->createTo();
473  to->setId(id2);
474  addVertex(to);
475  doInit=1;
476  }
477  }
478  }
479  if (from && to) {
480  e->setVertex(0, from);
481  e->setVertex(1, to);
482  e->read(currentLine);
483  if (!addEdge(e)) {
484  cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token << " " << id1 << " <-> " << id2 << endl;
485  delete e;
486  } else {
487  switch (doInit){
488  case 1:
489  {
490  HyperGraph::VertexSet fromSet;
491  fromSet.insert(from);
492  e->initialEstimate(fromSet, to);
493  break;
494  }
495  case 2:
496  {
497  HyperGraph::VertexSet toSet;
498  toSet.insert(to);
499  e->initialEstimate(toSet, from);
500  break;
501  }
502  default:;
503  }
504  }
505  }
506  }
507  else {
508  vector<int> ids;
509  ids.resize(numV);
510  for (int l = 0; l < numV; ++l)
511  currentLine >> ids[l];
512  bool vertsOkay = true;
513  for (int l = 0; l < numV; ++l) {
514  e->setVertex(l, vertex(ids[l]));
515  if (e->vertex(l) == 0) {
516  vertsOkay = false;
517  break;
518  }
519  }
520  if (! vertsOkay) {
521  cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token;
522  for (int l = 0; l < numV; ++l) {
523  if (l > 0)
524  cerr << " <->";
525  cerr << " " << ids[l];
526  }
527  delete e;
528  } else {
529  bool r = e->read(currentLine);
530  if (!r || !addEdge(e)) {
531  cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token;
532  for (int l = 0; l < numV; ++l) {
533  if (l > 0)
534  cerr << " <->";
535  cerr << " " << ids[l];
536  }
537  delete e;
538  }
539  }
540  }
541  } else if (dynamic_cast<Data*>(element)) { // reading in the data packet for the vertex
542  //cerr << "read data packet " << token << " vertex " << previousVertex->id() << endl;
543  Data* d = static_cast<Data*>(element);
544  bool r = d->read(currentLine);
545  if (! r) {
546  cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for vertex " << previousVertex->id() << endl;
547  delete d;
548  previousData = 0;
549  } else if (previousData){
550  //cerr << "chaining" << endl;
551  previousData->setNext(d);
552  previousData = d;
553  //cerr << "done" << endl;
554  } else if (previousVertex){
555  //cerr << "embedding in vertex" << endl;
556  previousVertex->setUserData(d);
557  previousData = d;
558  previousVertex = 0;
559  //cerr << "done" << endl;
560  } else {
561  cerr << __PRETTY_FUNCTION__ << ": got data element, but no vertex available" << endl;
562  delete d;
563  previousData = 0;
564  }
565  }
566  } // while read line
567 
568  return true;
569 }
570 
571 bool OptimizableGraph::load(const char* filename, bool createEdges)
572 {
573  ifstream ifs(filename);
574  if (!ifs) {
575  cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl;
576  return false;
577  }
578  return load(ifs, createEdges);
579 }
580 
581 bool OptimizableGraph::save(const char* filename, int level) const
582 {
583  ofstream ofs(filename);
584  if (!ofs)
585  return false;
586  return save(ofs, level);
587 }
588 
589 bool OptimizableGraph::save(ostream& os, int level) const
590 {
591  if (! _parameters.write(os))
592  return false;
593  set<Vertex*, VertexIDCompare> verticesToSave;
594  for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
595  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
596  if (e->level() == level) {
597  for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
598  verticesToSave.insert(static_cast<OptimizableGraph::Vertex*>(*it));
599  }
600  }
601  }
602 
603  for (set<Vertex*, VertexIDCompare>::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){
604  OptimizableGraph::Vertex* v = *it;
605  saveVertex(os, v);
606  }
607 
608  EdgeContainer edgesToSave;
609  for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
610  const OptimizableGraph::Edge* e = dynamic_cast<const OptimizableGraph::Edge*>(*it);
611  if (e->level() == level)
612  edgesToSave.push_back(const_cast<Edge*>(e));
613  }
614  sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare());
615 
616  for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) {
617  OptimizableGraph::Edge* e = *it;
618  saveEdge(os, e);
619  }
620 
621  return os.good();
622 }
623 
624 
625 bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level)
626 {
627  if (! _parameters.write(os))
628  return false;
629 
630  for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){
631  OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
632  saveVertex(os, v);
633  }
634  for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
635  OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
636  if (e->level() != level)
637  continue;
638 
639  bool verticesInEdge = true;
640  for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
641  if (vset.find(*it) == vset.end()) {
642  verticesInEdge = false;
643  break;
644  }
645  }
646  if (! verticesInEdge)
647  continue;
648 
649  saveEdge(os, e);
650  }
651 
652  return os.good();
653 }
654 
655 bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset)
656 {
657  if (!_parameters.write(os))
658  return false;
659  std::set<OptimizableGraph::Vertex*> vset;
660  for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
661  HyperGraph::Edge* e = *it;
662  for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
663  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
664  vset.insert(v);
665  }
666  }
667 
668  for (std::set<OptimizableGraph::Vertex*>::const_iterator it=vset.begin(); it!=vset.end(); ++it){
669  OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
670  saveVertex(os, v);
671  }
672 
673  for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
674  OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
675  saveEdge(os, e);
676  }
677 
678  return os.good();
679 }
680 
682  for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){
684  if (vertex(v->id()))
685  continue;
687  v2->edges().clear();
688  v2->setHessianIndex(-1);
689  addVertex(v2);
690  }
691  for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){
693  OptimizableGraph::Edge* en = e->clone();
694  en->resize(e->vertices().size());
695  int cnt = 0;
696  for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
698  assert(v);
699  en->setVertex(cnt++, v);
700  }
701  addEdge(en);
702  }
703 }
704 
706  int maxDim=0;
707  for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){
708  const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second);
709  maxDim = (std::max)(maxDim, v->dimension());
710  }
711  return maxDim;
712 }
713 
714 void OptimizableGraph::setRenamedTypesFromString(const std::string& types)
715 {
716  Factory* factory = Factory::instance();
717  vector<string> typesMap = strSplit(types, ",");
718  for (size_t i = 0; i < typesMap.size(); ++i) {
719  vector<string> m = strSplit(typesMap[i], "=");
720  if (m.size() != 2) {
721  cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl;
722  continue;
723  }
724  string typeInFile = trim(m[0]);
725  string loadedType = trim(m[1]);
726  if (! factory->knowsTag(loadedType)) {
727  cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl;
728  continue;
729  }
730 
731  _renamedTypesLookup[typeInFile] = loadedType;
732  }
733 
734  cerr << "# load look up table" << endl;
735  for (std::map<std::string, std::string>::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) {
736  cerr << "#\t" << it->first << " -> " << it->second << endl;
737  }
738 }
739 
740 bool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims_) const
741 {
742  std::set<int> auxDims;
743  if (vertDims_.size() == 0) {
744  auxDims = dimensions();
745  }
746  const set<int>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
747  bool suitableSolver = true;
748  if (vertDims.size() == 2) {
749  if (solverProperty.requiresMarginalize) {
750  suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1;
751  }
752  else {
753  suitableSolver = solverProperty.poseDim == -1;
754  }
755  } else if (vertDims.size() == 1) {
756  suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1;
757  } else {
758  suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;
759  }
760  return suitableSolver;
761 }
762 
763 std::set<int> OptimizableGraph::dimensions() const
764 {
765  std::set<int> auxDims;
766  for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) {
767  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
768  auxDims.insert(v->dimension());
769  }
770  return auxDims;
771 }
772 
774 {
776  if (actions.size() > 0) {
778  for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
779  (*(*it))(this, &params);
780  }
781  }
782 }
783 
785 {
787  if (actions.size() > 0) {
789  for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
790  (*(*it))(this, &params);
791  }
792  }
793 }
794 
796 {
797  std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_POSTITERATION].insert(action);
798  return insertResult.second;
799 }
800 
802 {
803  std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_PREITERATION].insert(action);
804  return insertResult.second;
805 }
806 
808 {
809  return _graphActions[AT_PREITERATION].erase(action) > 0;
810 }
811 
813 {
814  return _graphActions[AT_POSTITERATION].erase(action) > 0;
815 }
816 
818 {
819  Factory* factory = Factory::instance();
820  string tag = factory->tag(v);
821  if (tag.size() > 0) {
822  os << tag << " " << v->id() << " ";
823  v->write(os);
824  os << endl;
825  Data* d=v->userData();
826  while (d) { // write the data packet for the vertex
827  tag = factory->tag(d);
828  if (tag.size() > 0) {
829  os << tag << " ";
830  d->write(os);
831  os << endl;
832  }
833  d=d->next();
834  }
835  if (v->fixed()) {
836  os << "FIX " << v->id() << endl;
837  }
838  return os.good();
839  }
840  return false;
841 }
842 
843 bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const
844 {
845  Factory* factory = Factory::instance();
846  string tag = factory->tag(e);
847  if (tag.size() > 0) {
848  os << tag << " ";
849  if (_edge_has_id)
850  os << e->id() << " ";
851  for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
852  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
853  os << v->id() << " ";
854  }
855  e->write(os);
856  os << endl;
857  return os.good();
858  }
859  return false;
860 }
861 
863 {
864  _parameters.clear();
865 }
866 
868 {
869  bool allEdgeOk = true;
870  SelfAdjointEigenSolver<MatrixXd> eigenSolver;
871  for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
872  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
873  Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension());
874  // test on symmetry
875  bool isSymmetric = information.transpose() == information;
876  bool okay = isSymmetric;
877  if (isSymmetric) {
878  // compute the eigenvalues
879  eigenSolver.compute(information, Eigen::EigenvaluesOnly);
880  bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
881  okay = okay && isSPD;
882  }
883  allEdgeOk = allEdgeOk && okay;
884  if (! okay) {
885  if (verbose) {
886  if (! isSymmetric)
887  cerr << "Information Matrix for an edge is not symmetric:";
888  else
889  cerr << "Information Matrix for an edge is not SPD:";
890  for (size_t i = 0; i < e->vertices().size(); ++i)
891  cerr << " " << e->vertex(i)->id();
892  if (isSymmetric)
893  cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose();
894  cerr << endl;
895  }
896  }
897  }
898  return allEdgeOk;
899 }
900 
902 {
903 # if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0)
904  Eigen::initParallel();
905 # endif
906  return true;
907 }
908 
909 } // end namespace
910 
d
virtual bool write(std::ostream &os) const =0
write the vertex to a stream
int id() const
returns the id
Definition: hyper_graph.h:103
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:140
#define __PRETTY_FUNCTION__
Definition: macros.h:95
void setHessianIndex(int ti)
set the temporary index of the vertex in the parameter blocks
const OptimizableGraph * graph() const
static Factory * instance()
return the instance
Definition: factory.cpp:61
virtual bool getEstimateData(double *estimate) const
int readLine(std::istream &is, std::stringstream &currentLine)
virtual int optimize(int iterations, bool online=false)
const Data * next() const
std::vector< std::string > _parameterTypes
int dimension() const
returns the dimensions of the error function
describe the properties of a solver
virtual void discardTop()=0
pop the last element from the stack, without restoring the current estimate
bool removePreIterationAction(HyperGraphAction *action)
remove an action that should no longer be execured before each iteration
std::bitset< HyperGraph::HGET_NUM_ELEMS > GraphElemBitset
Definition: hyper_graph.h:74
bool saveEdge(std::ostream &os, Edge *e) const
virtual void clearParameters()
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
HyperGraph::HyperGraphElement * construct(const std::string &tag) const
Definition: factory.cpp:147
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
bool removePostIterationAction(HyperGraphAction *action)
remove an action that should no longer be execured after each iteration
virtual bool read(std::istream &is)=0
read the vertex from a stream, i.e., the internal state of the vertex
#define CL_RED(s)
Definition: color_macros.h:46
virtual void pop()
pop (restore) the estimate of all variables from the stack
virtual void push()
push the estimate of all variables onto a stack
int landmarkDim
dimension of the landmar vertices (-1 if variable)
const Data * userData() const
the user data associated with this vertex
virtual bool read(std::istream &is)=0
read the data from a stream
int level() const
returns the level of the edge
bool addPostIterationAction(HyperGraphAction *action)
add an action to be executed after each iteration
int maxDimension() const
return the maximum dimension of all vertices in the graph
virtual int measurementDimension() const
static bool initMultiThreading()
void setRobustKernel(RobustKernel *ptr)
std::vector< std::string > strSplit(const std::string &str, const std::string &delimiters)
virtual bool setMinimalEstimateDataImpl(const double *)
virtual void postIteration(int)
called at the end of an iteration (argument is the number of the iteration)
std::string trim(const std::string &s)
void setVertex(size_t i, Vertex *v)
Definition: hyper_graph.h:148
virtual bool addEdge(Edge *e)
Definition: hyper_graph.cpp:99
const VertexIDMap & vertices() const
Definition: hyper_graph.h:177
base for all robust cost functions
Definition: robust_kernel.h:51
virtual const double * informationData() const =0
returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd> ...
void setUpdateNeeded(bool needUpdate=true)
Definition: cache.cpp:170
std::vector< Parameter ** > _parameters
virtual void setId(int id)
sets the id of the node in the graph be sure that the graph keeps consistent after changing the id ...
virtual bool setEstimateDataImpl(const double *)
std::vector< OptimizableGraph::Edge * > EdgeContainer
vector container for edges
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:90
int dimension() const
dimension of the estimated state belonging to this node
bool setParameterId(int argNum, int paramId)
const VertexContainer & vertices() const
Definition: hyper_graph.h:132
double chi2() const
returns the chi2 of the current configuration
const std::string & tag(const HyperGraph::HyperGraphElement *v) const
return the TAG given a vertex
Definition: factory.cpp:157
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
Definition: hyper_graph.h:106
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
virtual int estimateDimension() const
virtual void initialEstimate(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)=0
const EdgeSet & edges() const
Definition: hyper_graph.h:182
bool saveSubset(std::ostream &os, HyperGraph::VertexSet &vset, int level=0)
save a subgraph to a stream. Again uses the Factory system.
bool setMinimalEstimateData(const double *estimate)
void updateSize(const HyperGraph::Edge *e)
virtual bool setMeasurementData(const double *m)
std::vector< int > _parameterIds
create vertices and edges based on TAGs in, for example, a file
Definition: factory.h:49
virtual void push()=0
backup the position of the vertex to a stack
virtual void clear()
clears the graph and empties all structures.
virtual void pop()=0
restore the position of the vertex by retrieving the position from the stack
virtual int minimalEstimateDimension() const
virtual bool getMinimalEstimateData(double *estimate) const
bool knowsTag(const std::string &tag, int *elementType=0) const
Definition: factory.cpp:173
std::map< std::string, std::string > _renamedTypesLookup
order edges based on the internal ID, which is assigned to the edge in addEdge()
A general case Vertex for optimization.
abstract Vertex, your types must derive from that one
Definition: hyper_graph.h:97
OptimizableGraph()
empty constructor
void setRenamedTypesFromString(const std::string &types)
data packet for a vertex. Extend this class to store in the vertices the potential additional informa...
void addGraph(OptimizableGraph *g)
adds all edges and vertices of the graph g to this graph.
virtual bool addVertex(Vertex *v)
Definition: hyper_graph.cpp:76
virtual void resize(size_t size)
Definition: hyper_graph.cpp:50
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...
bool setEstimateData(const double *estimate)
Parameter * parameter(int id)
JacobianWorkspace _jacobianWorkspace
virtual void setFixed(HyperGraph::VertexSet &vset, bool fixed)
fixes/releases a set of vertices
bool saveVertex(std::ostream &os, Vertex *v) const
virtual void preIteration(int)
called at the beginning of an iteration (argument is the number of the iteration) ...
bool fixed() const
true => this node is fixed during the optimization
std::set< HyperGraphAction * > HyperGraphActionSet
virtual Vertex * clone() const
returns a deep copy of the current vertex
virtual bool getMeasurementData(double *m) const
virtual bool write(std::ostream &os) const =0
write the vertex to a stream
bool requiresMarginalize
whether the solver requires marginalization of landmarks
virtual void discardTop()
discard the last backup of the estimate for all variables by removing it from the stack ...
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
virtual bool write(std::ostream &os) const =0
write the data to a stream
bool verifyInformationMatrices(bool verbose=false) const
virtual bool read(std::istream &is)=0
read the vertex from a stream, i.e., the internal state of the vertex
std::vector< HyperGraphActionSet > _graphActions
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
virtual Edge * clone() const
std::set< int > dimensions() const
int poseDim
dimension of the pose vertices (-1 if variable)
bool addPreIterationAction(HyperGraphAction *action)
add an action to be executed before each iteration
Abstract action that operates on an entire graph.
VertexContainer _vertices
Definition: hyper_graph.h:154


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47