optimizable_graph.h
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 #ifndef G2O_AIS_OPTIMIZABLE_GRAPH_HH_
28 #define G2O_AIS_OPTIMIZABLE_GRAPH_HH_
29 
30 #include <set>
31 #include <iostream>
32 #include <list>
33 #include <limits>
34 #include <cmath>
35 #include <typeinfo>
36 
37 #include "openmp_mutex.h"
38 #include "hyper_graph.h"
39 #include "parameter.h"
40 #include "parameter_container.h"
41 #include "jacobian_workspace.h"
42 
43 #include "../stuff/macros.h"
44 
45 namespace g2o {
46 
47  class HyperGraphAction;
48  struct OptimizationAlgorithmProperty;
49  class Cache;
50  class CacheContainer;
51  class RobustKernel;
52 
64  struct OptimizableGraph : public HyperGraph {
65 
66  enum ActionType {
68  AT_NUM_ELEMENTS, // keep as last element
69  };
70 
71  typedef std::set<HyperGraphAction*> HyperGraphActionSet;
72 
73  // forward declarations
74  class Vertex;
75  class Edge;
76 
82  {
83  friend struct OptimizableGraph;
84  public:
85  virtual ~Data();
86  Data();
88  virtual bool read(std::istream& is) = 0;
90  virtual bool write(std::ostream& os) const = 0;
92  const Data* next() const {return _next;}
93  Data* next() {return _next;}
94  void setNext(Data* next_) { _next = next_; }
95  protected:
96  Data* _next; // linked list of multiple data;
97  };
98 
103  bool operator() (const Vertex* v1, const Vertex* v2) const
104  {
105  return v1->id() < v2->id();
106  }
107  };
108 
112  struct EdgeIDCompare {
113  bool operator() (const Edge* e1, const Edge* e2) const
114  {
115  return e1->internalId() < e2->internalId();
116  }
117  };
118 
120  typedef std::vector<OptimizableGraph::Vertex*> VertexContainer;
122  typedef std::vector<OptimizableGraph::Edge*> EdgeContainer;
123 
127  class Vertex : public HyperGraph::Vertex {
128  private:
129  friend struct OptimizableGraph;
130  public:
131  Vertex();
132 
134  virtual Vertex* clone() const ;
135 
137  const Data* userData() const { return _userData; }
138  Data* userData() { return _userData; }
139 
140  void setUserData(Data* obs) { _userData = obs;}
141  void addUserData(Data* obs) {
142  if (obs) {
143  obs->setNext(_userData);
144  _userData=obs;
145  }
146  }
147 
148  virtual ~Vertex();
149 
151  void setToOrigin() { setToOriginImpl(); updateCache();}
152 
154  virtual const double& hessian(int i, int j) const = 0;
155  virtual double& hessian(int i, int j) = 0;
156  virtual double hessianDeterminant() const = 0;
157  virtual double* hessianData() = 0;
158 
160  virtual void mapHessianMemory(double* d) = 0;
161 
166  virtual int copyB(double* b_) const = 0;
167 
169  virtual const double& b(int i) const = 0;
170  virtual double& b(int i) = 0;
172  virtual double* bData() = 0;
173 
177  virtual void clearQuadraticForm() = 0;
178 
183  virtual double solveDirect(double lambda=0) = 0;
184 
190  bool setEstimateData(const double* estimate);
191 
197  bool setEstimateData(const std::vector<double>& estimate) {
198 #ifndef NDEBUG
199  int dim = estimateDimension();
200  assert((dim == -1) || (estimate.size() == std::size_t(dim)));
201 #endif
202  return setEstimateData(&estimate[0]);
203  };
204 
209  virtual bool getEstimateData(double* estimate) const;
210 
215  virtual bool getEstimateData(std::vector<double>& estimate) const {
216  int dim = estimateDimension();
217  if (dim < 0)
218  return false;
219  estimate.resize(dim);
220  return getEstimateData(&estimate[0]);
221  };
222 
227  virtual int estimateDimension() const;
228 
234  bool setMinimalEstimateData(const double* estimate);
235 
241  bool setMinimalEstimateData(const std::vector<double>& estimate) {
242 #ifndef NDEBUG
243  int dim = minimalEstimateDimension();
244  assert((dim == -1) || (estimate.size() == std::size_t(dim)));
245 #endif
246  return setMinimalEstimateData(&estimate[0]);
247  };
248 
253  virtual bool getMinimalEstimateData(double* estimate) const ;
254 
259  virtual bool getMinimalEstimateData(std::vector<double>& estimate) const {
260  int dim = minimalEstimateDimension();
261  if (dim < 0)
262  return false;
263  estimate.resize(dim);
264  return getMinimalEstimateData(&estimate[0]);
265  };
266 
271  virtual int minimalEstimateDimension() const;
272 
274  virtual void push() = 0;
275 
277  virtual void pop() = 0;
278 
280  virtual void discardTop() = 0;
281 
283  virtual int stackSize() const = 0;
284 
291  void oplus(const double* v)
292  {
293  oplusImpl(v);
294  updateCache();
295  }
296 
298  int hessianIndex() const { return _hessianIndex;}
299  int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();}
301  void setHessianIndex(int ti) { _hessianIndex = ti;}
302  void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);}
303 
305  bool fixed() const {return _fixed;}
307  void setFixed(bool fixed) { _fixed = fixed;}
308 
310  bool marginalized() const {return _marginalized;}
312  void setMarginalized(bool marginalized) { _marginalized = marginalized;}
313 
315  int dimension() const { return _dimension;}
316 
318  virtual void setId(int id) {_id = id;}
319 
321  void setColInHessian(int c) { _colInHessian = c;}
323  int colInHessian() const {return _colInHessian;}
324 
325  const OptimizableGraph* graph() const {return _graph;}
326 
327  OptimizableGraph* graph() {return _graph;}
328 
333  void lockQuadraticForm() { _quadraticFormMutex.lock();}
337  void unlockQuadraticForm() { _quadraticFormMutex.unlock();}
338 
340  virtual bool read(std::istream& is) = 0;
342  virtual bool write(std::ostream& os) const = 0;
343 
344  virtual void updateCache();
345 
346  CacheContainer* cacheContainer();
347  protected:
351  bool _fixed;
356 
358 
363  virtual void oplusImpl(const double* v) = 0;
364 
366  virtual void setToOriginImpl() = 0;
367 
372  virtual bool setEstimateDataImpl(const double* ) { return false;}
373 
378  virtual bool setMinimalEstimateDataImpl(const double* ) { return false;}
379 
380  };
381 
382  class Edge: public HyperGraph::Edge {
383  private:
384  friend struct OptimizableGraph;
385  public:
386  Edge();
387  virtual ~Edge();
388  virtual Edge* clone() const;
389 
390  // indicates if all vertices are fixed
391  virtual bool allVerticesFixed() const = 0;
392 
393  // computes the error of the edge and stores it in an internal structure
394  virtual void computeError() = 0;
395 
398  virtual bool setMeasurementData(const double* m);
399 
402  virtual bool getMeasurementData(double* m) const;
403 
406  virtual int measurementDimension() const;
407 
412  virtual bool setMeasurementFromState();
413 
415  RobustKernel* robustKernel() const { return _robustKernel;}
419  void setRobustKernel(RobustKernel* ptr);
420 
422  virtual const double* errorData() const = 0;
423  virtual double* errorData() = 0;
424 
426  virtual const double* informationData() const = 0;
427  virtual double* informationData() = 0;
428 
430  virtual double chi2() const = 0;
431 
438  virtual void constructQuadraticForm() = 0;
439 
448  virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0;
449 
454  virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;
455 
457  virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0;
458 
464  virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;}
465 
467  int level() const { return _level;}
469  void setLevel(int l) { _level=l;}
470 
472  int dimension() const { return _dimension;}
473 
474  virtual Vertex* createFrom() {return 0;}
475  virtual Vertex* createTo() {return 0;}
476 
478  virtual bool read(std::istream& is) = 0;
480  virtual bool write(std::ostream& os) const = 0;
481 
483  long long internalId() const { return _internalId;}
484 
485  OptimizableGraph* graph();
486  const OptimizableGraph* graph() const;
487 
488  bool setParameterId(int argNum, int paramId);
489  inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);}
490  inline size_t numParameters() const {return _parameters.size();}
491  inline void resizeParameters(size_t newSize) {
492  _parameters.resize(newSize, 0);
493  _parameterIds.resize(newSize, -1);
494  _parameterTypes.resize(newSize, typeid(void*).name());
495  }
496  protected:
498  int _level;
500  long long _internalId;
501 
502  std::vector<int> _cacheIds;
503 
504  template <typename ParameterType>
505  bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){
506  if (argNo>=_parameters.size())
507  return false;
508  _parameterIds[argNo] = paramId;
509  _parameters[argNo] = (Parameter**)&p;
510  _parameterTypes[argNo] = typeid(ParameterType).name();
511  return true;
512  }
513 
514  template <typename CacheType>
515  void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*,
516  const std::string& _type,
517  const ParameterVector& parameters);
518 
519  bool resolveParameters();
520  virtual bool resolveCaches();
521 
522  std::vector<std::string> _parameterTypes;
523  std::vector<Parameter**> _parameters;
524  std::vector<int> _parameterIds;
525  };
526 
528  inline Vertex* vertex(int id) { return reinterpret_cast<Vertex*>(HyperGraph::vertex(id));}
529 
531  inline const Vertex* vertex (int id) const{ return reinterpret_cast<const Vertex*>(HyperGraph::vertex(id));}
532 
535  virtual ~OptimizableGraph();
536 
538  void addGraph(OptimizableGraph* g);
539 
544  virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);
545  virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}
546 
552  virtual bool addEdge(HyperGraph::Edge* e);
553 
555  double chi2() const;
556 
558  int maxDimension() const;
559 
563  std::set<int> dimensions() const;
564 
569  virtual int optimize(int iterations, bool online=false);
570 
572  virtual void preIteration(int);
574  virtual void postIteration(int);
575 
580 
585 
587  virtual void push();
589  virtual void pop();
591  virtual void discardTop();
592 
594  virtual bool load(std::istream& is, bool createEdges=true);
595  bool load(const char* filename, bool createEdges=true);
597  virtual bool save(std::ostream& os, int level = 0) const;
599  bool save(const char* filename, int level = 0) const;
600 
601 
603  bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0);
604 
606  bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset);
607 
609  virtual void push(HyperGraph::VertexSet& vset);
611  virtual void pop(HyperGraph::VertexSet& vset);
613  virtual void discardTop(HyperGraph::VertexSet& vset);
614 
616  virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed);
617 
623  void setRenamedTypesFromString(const std::string& types);
624 
630  bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims = std::set<int>()) const;
631 
635  virtual void clearParameters();
636 
638  return _parameters.addParameter(p);
639  }
640 
641  Parameter* parameter(int id) {
642  return _parameters.getParameter(id);
643  }
644 
651  bool verifyInformationMatrices(bool verbose = false) const;
652 
653  // helper functions to save an individual vertex
654  bool saveVertex(std::ostream& os, Vertex* v) const;
655 
656  // helper functions to save an individual edge
657  bool saveEdge(std::ostream& os, Edge* e) const;
661 
668  static bool initMultiThreading();
669 
670  protected:
671  std::map<std::string, std::string> _renamedTypesLookup;
672  long long _nextEdgeId;
673  std::vector<HyperGraphActionSet> _graphActions;
674 
675  // do not watch this. To be removed soon, or integrated in a nice way
677 
680  };
681 
686 } // end namespace
687 
688 #endif
virtual HyperGraph::HyperGraphElementType elementType() const
d
bool installParameter(ParameterType *&p, size_t argNo, int paramId=-1)
int id() const
returns the id
Definition: hyper_graph.h:103
std::vector< OptimizableGraph::Vertex * > VertexContainer
vector container for vertices
void setColInHessian(int c)
set the row of this vertex in the Hessian
filename
void setHessianIndex(int ti)
set the temporary index of the vertex in the parameter blocks
const OptimizableGraph * graph() const
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
void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti))
long long internalId() const
the internal ID of the edge
int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const)
bool removePreIterationAction(HyperGraphAction *action)
remove an action that should no longer be execured before each iteration
bool saveEdge(std::ostream &os, Edge *e) const
virtual void clearParameters()
Parameter * getParameter(int id)
return a parameter based on its ID
virtual bool getMinimalEstimateData(std::vector< double > &estimate) const
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
HyperGraphElementType
enum of all the types we have in our graphs
Definition: hyper_graph.h:65
bool removePostIterationAction(HyperGraphAction *action)
remove an action that should no longer be execured after each iteration
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
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
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
int hessianIndex() const
temporary index of this node in the parameter vector obtained from linearization
bool addPostIterationAction(HyperGraphAction *action)
add an action to be executed after each iteration
int maxDimension() const
return the maximum dimension of all vertices in the graph
Vertex * vertex(int id)
returns a vertex id in the hyper-graph, or 0 if the vertex id is not present
Definition: hyper_graph.cpp:60
static bool initMultiThreading()
virtual bool setMinimalEstimateDataImpl(const double *)
virtual void postIteration(int)
called at the end of an iteration (argument is the number of the iteration)
bool setMinimalEstimateData(const std::vector< double > &estimate)
base for all robust cost functions
Definition: robust_kernel.h:51
Vertex * vertex(int id)
returns the vertex number id appropriately casted
const JacobianWorkspace & jacobianWorkspace() const
JacobianWorkspace & jacobianWorkspace()
the workspace for storing the Jacobians of the graph
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
double chi2() const
returns the chi2 of the current configuration
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
int colInHessian() const
get the row of this vertex in the Hessian
RobustKernel * robustKernel() const
if NOT NULL, error of this edge will be robustifed with the kernel
bool saveSubset(std::ostream &os, HyperGraph::VertexSet &vset, int level=0)
save a subgraph to a stream. Again uses the Factory system.
std::vector< int > _parameterIds
std::vector< Parameter * > ParameterVector
Definition: parameter.h:52
order vertices based on their ID
std::map< std::string, std::string > _renamedTypesLookup
bool addParameter(Parameter *p)
const Parameter * parameter(int argNo) const
bool marginalized() const
true => this node is marginalized out during the optimization
order edges based on the internal ID, which is assigned to the edge in addEdge()
A general case Vertex for optimization.
action
virtual bool addVertex(HyperGraph::Vertex *v)
abstract Vertex, your types must derive from that one
Definition: hyper_graph.h:97
void setToOrigin()
sets the node to the origin (used in the multilevel stuff)
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.
void resizeParameters(size_t newSize)
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...
map id to parameters
Parameter * parameter(int id)
JacobianWorkspace _jacobianWorkspace
void setMarginalized(bool marginalized)
true => this node should be marginalized out during the optimization
virtual void setFixed(HyperGraph::VertexSet &vset, bool fixed)
fixes/releases a set of vertices
bool addParameter(Parameter *p)
add parameter to the container
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 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
const Vertex * vertex(int id) const
returns the vertex number id appropriately casted
void setLevel(int l)
sets the level of the edge
ParameterContainer _parameters
virtual bool write(std::ostream &os) const =0
write the data to a stream
bool verifyInformationMatrices(bool verbose=false) const
virtual bool getEstimateData(std::vector< double > &estimate) const
provide memory workspace for computing the Jacobians
std::vector< HyperGraphActionSet > _graphActions
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
bool setEstimateData(const std::vector< double > &estimate)
std::set< int > dimensions() const
bool addPreIterationAction(HyperGraphAction *action)
add an action to be executed before each iteration
Abstract action that operates on an entire graph.


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