27 #ifndef G2O_BASE_VERTEX_H 28 #define G2O_BASE_VERTEX_H 32 #include "../stuff/macros.h" 35 #include <Eigen/Dense> 36 #include <Eigen/Cholesky> 37 #include <Eigen/StdVector> 42 using namespace Eigen;
52 template <
int D,
typename T>
56 typedef std::stack<EstimateType,
57 std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > >
60 static const int Dimension = D;
62 typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned >
HessianBlockType;
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);}
70 virtual double*
hessianData() {
return const_cast<double*
>(_hessian.data());}
72 virtual void mapHessianMemory(
double*
d);
74 virtual int copyB(
double* b_)
const {
75 memcpy(b_, _b.data(), Dimension *
sizeof(double));
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();}
83 virtual void clearQuadraticForm();
87 virtual double solveDirect(
double lambda=0);
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;}
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();}
102 const EstimateType&
estimate()
const {
return _estimate;}
104 void setEstimate(
const EstimateType& et) { _estimate = et; updateCache();}
108 Matrix<double, D, 1>
_b;
112 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void push()
backup the position of the vertex to a stack
Matrix< double, D, 1 > & b()
return right hand side b of the constructed linear system
virtual void discardTop()
pop the last element from the stack, without restoring the current estimate
Matrix< double, D, 1 > _b
const HessianBlockType & A() const
virtual const double & hessian(int i, int j) const
get the element from the hessian matrix
virtual const double & b(int i) const
get the b vector element
virtual int copyB(double *b_) const
virtual double & b(int i)
virtual double * bData()
return a pointer to the b vector associated with this vertex
HessianBlockType & A()
return the hessian block associated with the vertex
HessianBlockType _hessian
std::stack< EstimateType, std::vector< EstimateType, Eigen::aligned_allocator< EstimateType > > > BackupStackType
Eigen::Map< Matrix< double, D, D >, Matrix< double, D, D >::Flags &AlignedBit?Aligned:Unaligned > HessianBlockType
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
A general case Vertex for optimization.
virtual void pop()
restore the position of the vertex by retrieving the position from the stack
const EstimateType & estimate() const
return the current estimate of the vertex
virtual double * hessianData()
const Matrix< double, D, 1 > & b() const
virtual int stackSize() const
return the stack size
virtual double & hessian(int i, int j)
virtual double hessianDeterminant() const