base_vertex.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_BASE_VERTEX_H
28 #define G2O_BASE_VERTEX_H
29 
30 #include "optimizable_graph.h"
31 #include "creators.h"
32 #include "../stuff/macros.h"
33 
34 #include <Eigen/Core>
35 #include <Eigen/Dense>
36 #include <Eigen/Cholesky>
37 #include <Eigen/StdVector>
38 #include <stack>
39 
40 namespace g2o {
41 
42  using namespace Eigen;
43 
44 
52  template <int D, typename T>
54  public:
55  typedef T EstimateType;
56  typedef std::stack<EstimateType,
57  std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > >
59 
60  static const int Dimension = D;
61 
62  typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
63 
64  public:
65  BaseVertex();
66 
67  virtual const double& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
68  virtual double& hessian(int i, int j) { assert(i<D && j<D); return _hessian(i,j);}
69  virtual double hessianDeterminant() const {return _hessian.determinant();}
70  virtual double* hessianData() { return const_cast<double*>(_hessian.data());}
71 
72  virtual void mapHessianMemory(double* d);
73 
74  virtual int copyB(double* b_) const {
75  memcpy(b_, _b.data(), Dimension * sizeof(double));
76  return Dimension;
77  }
78 
79  virtual const double& b(int i) const { assert(i < D); return _b(i);}
80  virtual double& b(int i) { assert(i < D); return _b(i);}
81  virtual double* bData() { return _b.data();}
82 
83  virtual void clearQuadraticForm();
84 
87  virtual double solveDirect(double lambda=0);
88 
90  Matrix<double, D, 1>& b() { return _b;}
91  const Matrix<double, D, 1>& b() const { return _b;}
93  HessianBlockType& A() { return _hessian;}
94  const HessianBlockType& A() const { return _hessian;}
95 
96  virtual void push() { _backup.push(_estimate);}
97  virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
98  virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
99  virtual int stackSize() const {return _backup.size();}
100 
102  const EstimateType& estimate() const { return _estimate;}
104  void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}
105 
106  protected:
107  HessianBlockType _hessian;
108  Matrix<double, D, 1> _b;
109  EstimateType _estimate;
111  public:
112  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
113 };
114 
115 #include "base_vertex.hpp"
116 
117 } // end namespace g2o
118 
119 
120 #endif
d
virtual void push()
backup the position of the vertex to a stack
Definition: base_vertex.h:96
BackupStackType _backup
Definition: base_vertex.h:110
Matrix< double, D, 1 > & b()
return right hand side b of the constructed linear system
Definition: base_vertex.h:90
virtual void discardTop()
pop the last element from the stack, without restoring the current estimate
Definition: base_vertex.h:98
Matrix< double, D, 1 > _b
Definition: base_vertex.h:108
const HessianBlockType & A() const
Definition: base_vertex.h:94
virtual const double & hessian(int i, int j) const
get the element from the hessian matrix
Definition: base_vertex.h:67
virtual const double & b(int i) const
get the b vector element
Definition: base_vertex.h:79
virtual int copyB(double *b_) const
Definition: base_vertex.h:74
Templatized BaseVertex.
Definition: base_vertex.h:53
virtual double & b(int i)
Definition: base_vertex.h:80
virtual double * bData()
return a pointer to the b vector associated with this vertex
Definition: base_vertex.h:81
HessianBlockType & A()
return the hessian block associated with the vertex
Definition: base_vertex.h:93
HessianBlockType _hessian
Definition: base_vertex.h:107
std::stack< EstimateType, std::vector< EstimateType, Eigen::aligned_allocator< EstimateType > > > BackupStackType
Definition: base_vertex.h:58
Eigen::Map< Matrix< double, D, D >, Matrix< double, D, D >::Flags &AlignedBit?Aligned:Unaligned > HessianBlockType
Definition: base_vertex.h:62
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
Definition: base_vertex.h:104
A general case Vertex for optimization.
virtual void pop()
restore the position of the vertex by retrieving the position from the stack
Definition: base_vertex.h:97
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:102
virtual double * hessianData()
Definition: base_vertex.h:70
const Matrix< double, D, 1 > & b() const
Definition: base_vertex.h:91
virtual int stackSize() const
return the stack size
Definition: base_vertex.h:99
EstimateType _estimate
Definition: base_vertex.h:109
virtual double & hessian(int i, int j)
Definition: base_vertex.h:68
virtual double hessianDeterminant() const
Definition: base_vertex.h:69


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