Templatized BaseVertex. More...
#include <base_vertex.h>
Public Types | |
typedef std::stack < EstimateType, std::deque < EstimateType, Eigen::aligned_allocator < EstimateType > > > | BackupStackType |
typedef T | EstimateType |
typedef Map< Matrix< double, D, D >, Matrix< double, D, D > ::Flags &AlignedBit?Aligned:Unaligned > | HessianBlockType |
Public Member Functions | |
HessianBlockType & | A () |
return the hessian block associated with the vertex | |
const HessianBlockType & | A () const |
virtual const double & | b (int i) const |
get the b vector element | |
virtual double & | b (int i) |
Matrix< double, D, 1 > & | b () |
get the i_th element of the estimate | |
const Matrix< double, D, 1 > & | b () const |
BaseVertex () | |
virtual double * | bData () |
return a pointer to the b vector associated with this vertex | |
virtual void | clearQuadraticForm () |
virtual int | copyB (double *b_) const |
virtual void | discardTop () |
pop the last element from the stack, without restoring the current estimate | |
const EstimateType & | estimate () const |
return the current estimate of the vertex | |
EstimateType & | estimate () |
virtual const double & | hessian (int i, int j) const |
get the element from the hessian matrix | |
virtual double & | hessian (int i, int j) |
virtual double * | hessianData () |
virtual double | hessianDeterminant () const |
virtual void | mapHessianMemory (double *d) |
virtual void | pop () |
restore the position of the vertex by retrieving the position from the stack | |
virtual void | push () |
backup the position of the vertex to a stack | |
void | setEstimate (const EstimateType &et) |
set the estimate for the vertex | |
virtual void | setUncertainty (double *c) |
sets the covariance/information of the node, used internally | |
void | setUncertainty (const Matrix< double, D, D > &uncertainty) |
set the uncertainty of the vertex, i.e., marginal covariance | |
virtual double | solveDirect (double lambda=0) |
virtual int | stackSize () const |
return the stack size | |
const Matrix< double, D, D > & | uncertainty () const |
return the uncertainty, i.e., marginal covariance of the node | |
virtual double * | uncertaintyData () |
return the pointer to the uncertainty (marginal covariance) | |
Static Public Attributes | |
static const int | Dimension = D |
dimension of the estimate (minimal) in the manifold space | |
Protected Attributes | |
Matrix< double, D, 1 > | _b |
BackupStackType | _backup |
EstimateType | _estimate |
HessianBlockType | _hessian |
Matrix< double, D, D > | _uncertainty |
Templatized BaseVertex.
Templatized BaseVertex D : minimal dimension of the vertex, e.g., 3 for rotation in 3D T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D ID : dimension of the internal representaion, defaults to D, e.g., 4 for the Quaternion in 3D
Definition at line 45 of file base_vertex.h.
typedef std::stack< EstimateType, std::deque <EstimateType, Eigen::aligned_allocator<EstimateType> > > g2o::BaseVertex< D, T >::BackupStackType |
Definition at line 52 of file base_vertex.h.
typedef T g2o::BaseVertex< D, T >::EstimateType |
Definition at line 47 of file base_vertex.h.
typedef Map< Matrix<double, D, D>, Matrix<double, D, D>::Flags & AlignedBit ? Aligned : Unaligned > g2o::BaseVertex< D, T >::HessianBlockType |
Definition at line 61 of file base_vertex.h.
BaseVertex::BaseVertex | ( | ) |
Definition at line 20 of file base_vertex.h.
HessianBlockType& g2o::BaseVertex< D, T >::A | ( | ) | [inline] |
return the hessian block associated with the vertex
Definition at line 119 of file base_vertex.h.
const HessianBlockType& g2o::BaseVertex< D, T >::A | ( | ) | const [inline] |
Definition at line 120 of file base_vertex.h.
virtual const double& g2o::BaseVertex< D, T >::b | ( | int | i | ) | const [inline, virtual] |
get the b vector element
Implements g2o::OptimizableGraph::Vertex.
Definition at line 90 of file base_vertex.h.
virtual double& g2o::BaseVertex< D, T >::b | ( | int | i | ) | [inline, virtual] |
Implements g2o::OptimizableGraph::Vertex.
Definition at line 96 of file base_vertex.h.
get the i_th element of the estimate
return right hand side b of the constructed linear system
Definition at line 115 of file base_vertex.h.
const Matrix<double, D, 1>& g2o::BaseVertex< D, T >::b | ( | ) | const [inline] |
Definition at line 116 of file base_vertex.h.
virtual double* g2o::BaseVertex< D, T >::bData | ( | ) | [inline, virtual] |
return a pointer to the b vector associated with this vertex
Implements g2o::OptimizableGraph::Vertex.
Definition at line 102 of file base_vertex.h.
void BaseVertex::clearQuadraticForm | ( | ) | [virtual] |
set the b vector part of this vertex to zero
Implements g2o::OptimizableGraph::Vertex.
Definition at line 53 of file base_vertex.h.
virtual int g2o::BaseVertex< D, T >::copyB | ( | double * | b_ | ) | const [inline, virtual] |
copies the b vector in the array b_
Implements g2o::OptimizableGraph::Vertex.
Definition at line 84 of file base_vertex.h.
virtual void g2o::BaseVertex< D, T >::discardTop | ( | ) | [inline, virtual] |
pop the last element from the stack, without restoring the current estimate
Implements g2o::OptimizableGraph::Vertex.
Definition at line 131 of file base_vertex.h.
const EstimateType& g2o::BaseVertex< D, T >::estimate | ( | ) | const [inline] |
return the current estimate of the vertex
Definition at line 140 of file base_vertex.h.
EstimateType& g2o::BaseVertex< D, T >::estimate | ( | ) | [inline] |
Definition at line 141 of file base_vertex.h.
virtual const double& g2o::BaseVertex< D, T >::hessian | ( | int | i, |
int | j | ||
) | const [inline, virtual] |
get the element from the hessian matrix
Implements g2o::OptimizableGraph::Vertex.
Definition at line 66 of file base_vertex.h.
virtual double& g2o::BaseVertex< D, T >::hessian | ( | int | i, |
int | j | ||
) | [inline, virtual] |
Implements g2o::OptimizableGraph::Vertex.
Definition at line 72 of file base_vertex.h.
virtual double* g2o::BaseVertex< D, T >::hessianData | ( | ) | [inline, virtual] |
Implements g2o::OptimizableGraph::Vertex.
Definition at line 80 of file base_vertex.h.
virtual double g2o::BaseVertex< D, T >::hessianDeterminant | ( | ) | const [inline, virtual] |
Implements g2o::OptimizableGraph::Vertex.
Definition at line 78 of file base_vertex.h.
void BaseVertex::mapHessianMemory | ( | double * | d | ) | [virtual] |
maps the internal matrix to some external memory location
Implements g2o::OptimizableGraph::Vertex.
Definition at line 59 of file base_vertex.h.
virtual void g2o::BaseVertex< D, T >::pop | ( | ) | [inline, virtual] |
restore the position of the vertex by retrieving the position from the stack
Implements g2o::OptimizableGraph::Vertex.
Reimplemented in g2o::VertexSE3AxisAngle, and g2o::VertexSE3AxisAngle.
Definition at line 124 of file base_vertex.h.
virtual void g2o::BaseVertex< D, T >::push | ( | ) | [inline, virtual] |
backup the position of the vertex to a stack
Implements g2o::OptimizableGraph::Vertex.
Reimplemented in g2o::VertexSE3AxisAngle, and g2o::VertexSE3AxisAngle.
Definition at line 122 of file base_vertex.h.
void g2o::BaseVertex< D, T >::setEstimate | ( | const EstimateType & | et | ) | [inline] |
set the estimate for the vertex
Definition at line 143 of file base_vertex.h.
void g2o::BaseVertex< D, T >::setUncertainty | ( | double * | c | ) | [virtual] |
sets the covariance/information of the node, used internally
Implements g2o::OptimizableGraph::Vertex.
Definition at line 29 of file base_vertex.h.
void g2o::BaseVertex< D, T >::setUncertainty | ( | const Matrix< double, D, D > & | uncertainty | ) | [inline] |
set the uncertainty of the vertex, i.e., marginal covariance
Definition at line 150 of file base_vertex.h.
double BaseVertex::solveDirect | ( | double | lambda = 0 | ) | [virtual] |
updates the current vertex with the direct solution x += H_ii
Implements g2o::OptimizableGraph::Vertex.
Definition at line 38 of file base_vertex.h.
virtual int g2o::BaseVertex< D, T >::stackSize | ( | ) | const [inline, virtual] |
return the stack size
Implements g2o::OptimizableGraph::Vertex.
Definition at line 137 of file base_vertex.h.
const Matrix<double, D, D>& g2o::BaseVertex< D, T >::uncertainty | ( | ) | const [inline] |
return the uncertainty, i.e., marginal covariance of the node
Definition at line 148 of file base_vertex.h.
virtual double* g2o::BaseVertex< D, T >::uncertaintyData | ( | ) | [inline, virtual] |
return the pointer to the uncertainty (marginal covariance)
Implements g2o::OptimizableGraph::Vertex.
Definition at line 146 of file base_vertex.h.
Definition at line 155 of file base_vertex.h.
BackupStackType g2o::BaseVertex< D, T >::_backup [protected] |
Definition at line 157 of file base_vertex.h.
EstimateType g2o::BaseVertex< D, T >::_estimate [protected] |
Definition at line 156 of file base_vertex.h.
HessianBlockType g2o::BaseVertex< D, T >::_hessian [protected] |
Definition at line 154 of file base_vertex.h.
Matrix<double, D, D> g2o::BaseVertex< D, T >::_uncertainty [protected] |
Definition at line 158 of file base_vertex.h.
const int g2o::BaseVertex< D, T >::Dimension = D [static] |
dimension of the estimate (minimal) in the manifold space
Definition at line 55 of file base_vertex.h.