g2o
Public Types | Public Member Functions | Static Public Attributes | Protected Attributes | List of all members
g2o::BaseVertex< D, T > Class Template Reference

Templatized BaseVertex. More...

#include <base_vertex.h>

Inheritance diagram for g2o::BaseVertex< D, T >:
Inheritance graph
[legend]
Collaboration diagram for g2o::BaseVertex< D, T >:
Collaboration graph
[legend]

Public Types

typedef T EstimateType
 
typedef std::stack< EstimateType, std::vector< EstimateType, Eigen::aligned_allocator< EstimateType > > > BackupStackType
 
typedef Eigen::Map< Eigen::Matrix< double, D, D, Eigen::ColMajor >, Eigen::Matrix< double, D, D, Eigen::ColMajor >::Flags &Eigen::AlignedBit?Eigen::Aligned:Eigen::Unaligned > HessianBlockType
 

Public Member Functions

 BaseVertex ()
 
virtual const double & hessian (int i, int j) const
 get the element from the hessian matrix More...
 
virtual double & hessian (int i, int j)
 
virtual double hessianDeterminant () const
 
virtual double * hessianData ()
 
virtual void mapHessianMemory (double *d)
 
virtual int copyB (double *b_) const
 
virtual const double & b (int i) const
 get the b vector element More...
 
virtual double & b (int i)
 
virtual double * bData ()
 return a pointer to the b vector associated with this vertex More...
 
virtual void clearQuadraticForm ()
 
virtual double solveDirect (double lambda=0)
 
Eigen::Matrix< double, D, 1, Eigen::ColMajor > & b ()
 return right hand side b of the constructed linear system More...
 
const Eigen::Matrix< double, D, 1, Eigen::ColMajor > & b () const
 
HessianBlockTypeA ()
 return the hessian block associated with the vertex More...
 
const HessianBlockTypeA () const
 
virtual void push ()
 backup the position of the vertex to a stack More...
 
virtual void pop ()
 restore the position of the vertex by retrieving the position from the stack More...
 
virtual void discardTop ()
 pop the last element from the stack, without restoring the current estimate More...
 
virtual int stackSize () const
 return the stack size More...
 
const EstimateTypeestimate () const
 return the current estimate of the vertex More...
 
void setEstimate (const EstimateType &et)
 set the estimate for the vertex also calls updateCache() More...
 
- Public Member Functions inherited from g2o::OptimizableGraph::Vertex
 Vertex ()
 
virtual Vertexclone () const
 returns a deep copy of the current vertex More...
 
virtual ~Vertex ()
 
void setToOrigin ()
 sets the node to the origin (used in the multilevel stuff) More...
 
bool setEstimateData (const double *estimate)
 
bool setEstimateData (const std::vector< double > &estimate)
 
virtual bool getEstimateData (double *estimate) const
 
virtual bool getEstimateData (std::vector< double > &estimate) const
 
virtual int estimateDimension () const
 
bool setMinimalEstimateData (const double *estimate)
 
bool setMinimalEstimateData (const std::vector< double > &estimate)
 
virtual bool getMinimalEstimateData (double *estimate) const
 
virtual bool getMinimalEstimateData (std::vector< double > &estimate) const
 
virtual int minimalEstimateDimension () const
 
void oplus (const double *v)
 
int hessianIndex () const
 temporary index of this node in the parameter vector obtained from linearization More...
 
int G2O_ATTRIBUTE_DEPRECATED (tempIndex() const)
 
void setHessianIndex (int ti)
 set the temporary index of the vertex in the parameter blocks More...
 
void G2O_ATTRIBUTE_DEPRECATED (setTempIndex(int ti))
 
bool fixed () const
 true => this node is fixed during the optimization More...
 
void setFixed (bool fixed)
 true => this node should be considered fixed during the optimization More...
 
bool marginalized () const
 true => this node is marginalized out during the optimization More...
 
void setMarginalized (bool marginalized)
 true => this node should be marginalized out during the optimization More...
 
int dimension () const
 dimension of the estimated state belonging to this node More...
 
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 More...
 
void setColInHessian (int c)
 set the row of this vertex in the Hessian More...
 
int colInHessian () const
 get the row of this vertex in the Hessian More...
 
const OptimizableGraphgraph () const
 
OptimizableGraphgraph ()
 
void lockQuadraticForm ()
 
void unlockQuadraticForm ()
 
virtual bool read (std::istream &is)=0
 read the vertex from a stream, i.e., the internal state of the vertex More...
 
virtual bool write (std::ostream &os) const =0
 write the vertex to a stream More...
 
virtual void updateCache ()
 
CacheContainercacheContainer ()
 
- Public Member Functions inherited from g2o::HyperGraph::Vertex
 Vertex (int id=InvalidId)
 creates a vertex having an ID specified by the argument More...
 
int id () const
 returns the id More...
 
const EdgeSetedges () const
 returns the set of hyper-edges that are leaving/entering in this vertex More...
 
EdgeSetedges ()
 returns the set of hyper-edges that are leaving/entering in this vertex More...
 
virtual HyperGraphElementType elementType () const
 
- Public Member Functions inherited from g2o::HyperGraph::HyperGraphElement
virtual ~HyperGraphElement ()
 
HyperGraphElementclone () const
 
- Public Member Functions inherited from g2o::HyperGraph::DataContainer
 DataContainer ()
 
virtual ~DataContainer ()
 
const DatauserData () const
 the user data associated with this vertex More...
 
DatauserData ()
 
void setUserData (Data *obs)
 
void addUserData (Data *obs)
 

Static Public Attributes

static const int Dimension = D
 dimension of the estimate (minimal) in the manifold space More...
 

Protected Attributes

HessianBlockType _hessian
 
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
 
EstimateType _estimate
 
BackupStackType _backup
 
- Protected Attributes inherited from g2o::OptimizableGraph::Vertex
OptimizableGraph_graph
 
Data_userData
 
int _hessianIndex
 
bool _fixed
 
bool _marginalized
 
int _dimension
 
int _colInHessian
 
OpenMPMutex _quadraticFormMutex
 
CacheContainer_cacheContainer
 
- Protected Attributes inherited from g2o::HyperGraph::Vertex
int _id
 
EdgeSet _edges
 
- Protected Attributes inherited from g2o::HyperGraph::DataContainer
Data_userData
 

Additional Inherited Members

- Protected Member Functions inherited from g2o::OptimizableGraph::Vertex
virtual void oplusImpl (const double *v)=0
 
virtual void setToOriginImpl ()=0
 sets the node to the origin (used in the multilevel stuff) More...
 
virtual bool setEstimateDataImpl (const double *)
 
virtual bool setMinimalEstimateDataImpl (const double *)
 

Detailed Description

template<int D, typename T>
class g2o::BaseVertex< D, T >

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

Definition at line 50 of file base_vertex.h.

Member Typedef Documentation

template<int D, typename T>
typedef std::stack<EstimateType, std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > > g2o::BaseVertex< D, T >::BackupStackType

Definition at line 55 of file base_vertex.h.

template<int D, typename T>
typedef T g2o::BaseVertex< D, T >::EstimateType

Definition at line 52 of file base_vertex.h.

template<int D, typename T>
typedef Eigen::Map<Eigen::Matrix<double, D, D, Eigen::ColMajor>, Eigen::Matrix<double, D, D, Eigen::ColMajor>::Flags & Eigen::AlignedBit ? Eigen::Aligned : Eigen::Unaligned > g2o::BaseVertex< D, T >::HessianBlockType

Definition at line 59 of file base_vertex.h.

Constructor & Destructor Documentation

template<int D, typename T >
BaseVertex::BaseVertex ( )

Definition at line 29 of file base_vertex.h.

40  {

Member Function Documentation

template<int D, typename T>
HessianBlockType& g2o::BaseVertex< D, T >::A ( )
inline

return the hessian block associated with the vertex

Definition at line 90 of file base_vertex.h.

90 { return _hessian;}
HessianBlockType _hessian
Definition: base_vertex.h:104
template<int D, typename T>
const HessianBlockType& g2o::BaseVertex< D, T >::A ( ) const
inline

Definition at line 91 of file base_vertex.h.

91 { return _hessian;}
HessianBlockType _hessian
Definition: base_vertex.h:104
template<int D, typename T>
virtual const double& g2o::BaseVertex< D, T >::b ( int  i) const
inlinevirtual

get the b vector element

Implements g2o::OptimizableGraph::Vertex.

Definition at line 76 of file base_vertex.h.

76 { assert(i < D); return _b(i);}
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
Definition: base_vertex.h:105
template<int D, typename T>
virtual double& g2o::BaseVertex< D, T >::b ( int  i)
inlinevirtual

Implements g2o::OptimizableGraph::Vertex.

Definition at line 77 of file base_vertex.h.

77 { assert(i < D); return _b(i);}
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
Definition: base_vertex.h:105
template<int D, typename T>
Eigen::Matrix<double, D, 1, Eigen::ColMajor>& g2o::BaseVertex< D, T >::b ( )
inline

return right hand side b of the constructed linear system

Definition at line 87 of file base_vertex.h.

87 { return _b;}
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
Definition: base_vertex.h:105
template<int D, typename T>
const Eigen::Matrix<double, D, 1, Eigen::ColMajor>& g2o::BaseVertex< D, T >::b ( ) const
inline

Definition at line 88 of file base_vertex.h.

88 { return _b;}
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
Definition: base_vertex.h:105
template<int D, typename T>
virtual double* g2o::BaseVertex< D, T >::bData ( )
inlinevirtual

return a pointer to the b vector associated with this vertex

Implements g2o::OptimizableGraph::Vertex.

Definition at line 78 of file base_vertex.h.

78 { return _b.data();}
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
Definition: base_vertex.h:105
template<int D, typename T >
void BaseVertex::clearQuadraticForm ( )
virtual

set the b vector part of this vertex to zero

Implements g2o::OptimizableGraph::Vertex.

Definition at line 48 of file base_vertex.h.

Referenced by g2o::BaseVertex< 6, Vector6d >::bData().

50  : public OptimizableGraph::Vertex {
class G2O_CORE_API Vertex
template<int D, typename T>
virtual int g2o::BaseVertex< D, T >::copyB ( double *  b_) const
inlinevirtual

copies the b vector in the array b_

Returns
the number of elements copied

Implements g2o::OptimizableGraph::Vertex.

Definition at line 71 of file base_vertex.h.

71  {
72  memcpy(b_, _b.data(), Dimension * sizeof(double));
73  return Dimension;
74  }
Eigen::Matrix< double, D, 1, Eigen::ColMajor > _b
Definition: base_vertex.h:105
static const int Dimension
dimension of the estimate (minimal) in the manifold space
Definition: base_vertex.h:57
template<int D, typename T>
virtual void g2o::BaseVertex< D, T >::discardTop ( )
inlinevirtual

pop the last element from the stack, without restoring the current estimate

Implements g2o::OptimizableGraph::Vertex.

Definition at line 95 of file base_vertex.h.

95 { assert(!_backup.empty()); _backup.pop();}
BackupStackType _backup
Definition: base_vertex.h:107
template<int D, typename T>
const EstimateType& g2o::BaseVertex< D, T >::estimate ( ) const
inline

return the current estimate of the vertex

Definition at line 99 of file base_vertex.h.

Referenced by g2o::OnlineEdgeSE3::chi2(), g2o::EdgeSE2TwoPointsXY::computeError(), g2o::EdgeSE3::computeError(), g2o::EdgeSE3LotsOfXYZ::computeError(), g2o::EdgeSE2LotsOfXY::computeError(), g2o::EdgePointXYZ::computeError(), g2o::EdgeLine2D::computeError(), g2o::EdgePointXY::computeError(), g2o::EdgeSE2PointXYBearing::computeError(), g2o::EdgeLine3D::computeError(), g2o::EdgePlane::computeError(), g2o::EdgeSE2PointXY::computeError(), g2o::EdgeLine2DPointXY::computeError(), g2o::EdgeSE2Line2D::computeError(), g2o::EdgeSE2SensorCalib::computeError(), g2o::EdgeSE2OdomDifferentialCalib::computeError(), g2o::EdgeSE2Prior::computeError(), g2o::EdgeSE3PlaneSensorCalib::computeError(), g2o::EdgeSE2::computeError(), g2o::EdgeSE2PointXYCalib::computeError(), g2o::EdgeSE3Calib::computeError(), g2o::tutorial::EdgeSE2::computeError(), g2o::EdgeSE2Segment2D::computeError(), g2o::EdgeSE3PointXYZDepth::computeError(), g2o::tutorial::EdgeSE2PointXY::computeError(), g2o::EdgeSE3PointXYZ::computeError(), g2o::EdgeSE2Segment2DLine::computeError(), g2o::deprecated::EdgeSE3PointXYZDepth::computeError(), g2o::EdgeSE3Line3D::computeError(), g2o::deprecated::EdgeSE3PointXYZ::computeError(), g2o::EdgeSE2PointXYOffset::computeError(), g2o::deprecated::EdgeSE3::computeError(), g2o::EdgeSE2Segment2DPointLine::computeError(), g2o::EdgeSE3PointXYZDisparity::computeError(), g2o::EdgeSE2PureCalib::computeError(), g2o::deprecated::EdgeSE3PointXYZDisparity::computeError(), g2o::EdgeSE2XYPrior::computeError(), EdgeCalib::computeError(), GPSObservationPosition3DEdge::computeError(), g2o::EdgeSim3::computeError(), EdgePointOnCurve::computeError(), EdgePointOnCircle::computeError(), g2o::EdgeSE3Expmap::computeError(), g2o::EdgeSim3ProjectXYZ::computeError(), TargetOdometry3DEdge::computeError(), g2o::EdgeProjectXYZ2UV::computeError(), g2o::EdgeProjectP2MC::computeError(), g2o::EdgeProjectPSI2UV::computeError(), GPSObservationEdgePositionVelocity3D::computeError(), g2o::Edge_V_V_GICP::computeError(), g2o::EdgeProjectXYZ2UVU::computeError(), g2o::EdgeProjectP2SC::computeError(), EdgeObservationBAL::computeError(), g2o::EdgeProjectP2MC_Intrinsics::computeError(), g2o::EdgeSBACam::computeError(), g2o::EdgeSBAScale::computeError(), g2o::Edge_XYZ_VSC::computeError(), g2o::Slam2DViewer::draw(), g2o::Edge_XYZ_VSC::Edge_XYZ_VSC(), gnudump_edges(), gnudump_features(), g2o::EdgeSE2TwoPointsXY::initialEstimate(), g2o::EdgeSE3LotsOfXYZ::initialEstimate(), g2o::EdgeSE2LotsOfXY::initialEstimate(), g2o::EdgeSE3::initialEstimate(), g2o::EdgeSE2PointXYCalib::initialEstimate(), g2o::EdgeSE3Offset::initialEstimate(), g2o::EdgeSE2SensorCalib::initialEstimate(), g2o::EdgeSE2PointXYBearing::initialEstimate(), g2o::deprecated::EdgeSE3Prior::initialEstimate(), g2o::EdgeSE3Prior::initialEstimate(), g2o::EdgeSE3PointXYZ::initialEstimate(), g2o::EdgeSE2Offset::initialEstimate(), g2o::deprecated::EdgeSE3Offset::initialEstimate(), g2o::EdgeSE3PointXYZDisparity::initialEstimate(), g2o::deprecated::EdgeSE3PointXYZDisparity::initialEstimate(), g2o::EdgeSE2::initialEstimate(), g2o::deprecated::EdgeSE3::initialEstimate(), g2o::EdgeSim3::initialEstimate(), TargetOdometry3DEdge::initialEstimate(), g2o::EdgeSBACam::initialEstimate(), g2o::EdgeSBAScale::initialEstimate(), g2o::EdgeSE3::linearizeOplus(), g2o::EdgeSE3LotsOfXYZ::linearizeOplus(), g2o::EdgeSE2LotsOfXY::linearizeOplus(), g2o::deprecated::EdgeSE3Prior::linearizeOplus(), g2o::EdgeSE3PointXYZDepth::linearizeOplus(), g2o::EdgeSE3Prior::linearizeOplus(), g2o::EdgeSE3PointXYZ::linearizeOplus(), g2o::deprecated::EdgeSE3PointXYZDepth::linearizeOplus(), g2o::EdgeSE3Offset::linearizeOplus(), g2o::deprecated::EdgeSE3PointXYZ::linearizeOplus(), g2o::EdgeSE2PointXYOffset::linearizeOplus(), g2o::EdgeSE3PointXYZDisparity::linearizeOplus(), g2o::deprecated::EdgeSE3PointXYZDisparity::linearizeOplus(), g2o::deprecated::EdgeSE3Offset::linearizeOplus(), g2o::EdgeSE2PointXY::linearizeOplus(), g2o::EdgeSE2::linearizeOplus(), g2o::deprecated::EdgeSE3::linearizeOplus(), g2o::EdgeSE3Expmap::linearizeOplus(), g2o::EdgeProjectXYZ2UV::linearizeOplus(), g2o::EdgeProjectPSI2UV::linearizeOplus(), g2o::EdgeProjectP2MC::linearizeOplus(), g2o::Edge_V_V_GICP::linearizeOplus(), g2o::EdgeProjectP2SC::linearizeOplus(), EdgeObservationBAL::linearizeOplus(), g2o::EdgeProjectP2MC_Intrinsics::linearizeOplus(), main(), Robot::move(), g2o::EdgeSE3WriteGnuplotAction::operator()(), g2o::EdgeSE2PointXYBearingWriteGnuplotAction::operator()(), g2o::EdgeSE2PointXYWriteGnuplotAction::operator()(), g2o::VertexPointXYZWriteGnuplotAction::operator()(), g2o::VertexPointXYWriteGnuplotAction::operator()(), g2o::VertexSE2WriteGnuplotAction::operator()(), g2o::deprecated::VertexPointXYZWriteGnuplotAction::operator()(), g2o::EdgeSE2WriteGnuplotAction::operator()(), g2o::deprecated::VertexSE3WriteGnuplotAction::operator()(), g2o::deprecated::EdgeSE3WriteGnuplotAction::operator()(), g2o::VertexSE3WriteGnuplotAction::operator()(), g2o::Gm2dlIO::readGm2dl(), PlaneSensor::sense(), g2o::EdgeSE2TwoPointsXY::setMeasurementFromState(), g2o::EdgeSE3LotsOfXYZ::setMeasurementFromState(), g2o::EdgeSE2LotsOfXY::setMeasurementFromState(), g2o::EdgeSE3::setMeasurementFromState(), g2o::EdgeSE2PointXYBearing::setMeasurementFromState(), g2o::EdgeSE2PointXY::setMeasurementFromState(), g2o::EdgeLine2DPointXY::setMeasurementFromState(), g2o::EdgePointXYZ::setMeasurementFromState(), g2o::EdgePointXY::setMeasurementFromState(), g2o::EdgePlane::setMeasurementFromState(), g2o::EdgeLine3D::setMeasurementFromState(), g2o::EdgeSE3PointXYZDepth::setMeasurementFromState(), g2o::EdgeSE2Line2D::setMeasurementFromState(), g2o::EdgeLine2D::setMeasurementFromState(), g2o::deprecated::EdgeSE3PointXYZDepth::setMeasurementFromState(), g2o::EdgeSE3PointXYZ::setMeasurementFromState(), g2o::EdgeSE2Segment2D::setMeasurementFromState(), g2o::EdgeSE2PointXYOffset::setMeasurementFromState(), g2o::deprecated::EdgeSE3PointXYZ::setMeasurementFromState(), g2o::EdgeSE3PointXYZDisparity::setMeasurementFromState(), g2o::EdgeSE2::setMeasurementFromState(), g2o::deprecated::EdgeSE3PointXYZDisparity::setMeasurementFromState(), g2o::EdgeSE2Segment2DLine::setMeasurementFromState(), g2o::EdgeSE2Segment2DPointLine::setMeasurementFromState(), g2o::deprecated::EdgeSE3::setMeasurementFromState(), g2o::EdgeSBACam::setMeasurementFromState(), updateDisplay(), g2o::tutorial::CacheSE2Offset::updateImpl(), g2o::CacheSE3Offset::updateImpl(), g2o::CacheSE2Offset::updateImpl(), g2o::deprecated::CacheSE3Offset::updateImpl(), g2o::Gm2dlIO::updateLaserData(), g2o::VertexPointXYZ::write(), g2o::VertexPlane::write(), g2o::deprecated::VertexPointXYZ::write(), g2o::EdgeSE3PlaneSensorCalib::write(), g2o::EdgeSE2OdomDifferentialCalib::write(), g2o::EdgeSE2SensorCalib::write(), and g2o::VertexLine2D::write().

99 { return _estimate;}
EstimateType _estimate
Definition: base_vertex.h:106
template<int D, typename T>
virtual const double& g2o::BaseVertex< D, T >::hessian ( int  i,
int  j 
) const
inlinevirtual

get the element from the hessian matrix

Implements g2o::OptimizableGraph::Vertex.

Definition at line 64 of file base_vertex.h.

64 { assert(i<D && j<D); return _hessian(i,j);}
HessianBlockType _hessian
Definition: base_vertex.h:104
template<int D, typename T>
virtual double& g2o::BaseVertex< D, T >::hessian ( int  i,
int  j 
)
inlinevirtual

Implements g2o::OptimizableGraph::Vertex.

Definition at line 65 of file base_vertex.h.

65 { assert(i<D && j<D); return _hessian(i,j);}
HessianBlockType _hessian
Definition: base_vertex.h:104
template<int D, typename T>
virtual double* g2o::BaseVertex< D, T >::hessianData ( )
inlinevirtual

Implements g2o::OptimizableGraph::Vertex.

Definition at line 67 of file base_vertex.h.

67 { return const_cast<double*>(_hessian.data());}
HessianBlockType _hessian
Definition: base_vertex.h:104
template<int D, typename T>
virtual double g2o::BaseVertex< D, T >::hessianDeterminant ( ) const
inlinevirtual

Implements g2o::OptimizableGraph::Vertex.

Definition at line 66 of file base_vertex.h.

66 {return _hessian.determinant();}
HessianBlockType _hessian
Definition: base_vertex.h:104
template<int D, typename T >
void BaseVertex::mapHessianMemory ( double *  d)
virtual

maps the internal matrix to some external memory location

Implements g2o::OptimizableGraph::Vertex.

Definition at line 53 of file base_vertex.h.

Referenced by g2o::BaseVertex< 6, Vector6d >::hessianData().

template<int D, typename T>
virtual void g2o::BaseVertex< D, T >::pop ( )
inlinevirtual

restore the position of the vertex by retrieving the position from the stack

Implements g2o::OptimizableGraph::Vertex.

Definition at line 94 of file base_vertex.h.

94 { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
BackupStackType _backup
Definition: base_vertex.h:107
EstimateType _estimate
Definition: base_vertex.h:106
template<int D, typename T>
virtual void g2o::BaseVertex< D, T >::push ( )
inlinevirtual

backup the position of the vertex to a stack

Implements g2o::OptimizableGraph::Vertex.

Definition at line 93 of file base_vertex.h.

Referenced by g2o::findConnectedEdgesWithCostLimit().

93 { _backup.push(_estimate);}
BackupStackType _backup
Definition: base_vertex.h:107
EstimateType _estimate
Definition: base_vertex.h:106
template<int D, typename T>
void g2o::BaseVertex< D, T >::setEstimate ( const EstimateType et)
inline

set the estimate for the vertex also calls updateCache()

Definition at line 101 of file base_vertex.h.

Referenced by g2o::EdgeSE2TwoPointsXY::initialEstimate(), g2o::EdgeSE3LotsOfXYZ::initialEstimate(), g2o::EdgeSE2LotsOfXY::initialEstimate(), g2o::EdgeSE3::initialEstimate(), g2o::EdgeSE2PointXYCalib::initialEstimate(), g2o::EdgeSE3Offset::initialEstimate(), g2o::OnlineEdgeSE3::initialEstimate(), g2o::OnlineEdgeSE2::initialEstimate(), g2o::EdgeSE2SensorCalib::initialEstimate(), g2o::EdgeSE2PointXY::initialEstimate(), g2o::EdgeSE2Prior::initialEstimate(), g2o::EdgeSE2PointXYBearing::initialEstimate(), g2o::deprecated::EdgeSE3Prior::initialEstimate(), g2o::EdgeSE3PointXYZDepth::initialEstimate(), g2o::EdgeSE3Prior::initialEstimate(), g2o::deprecated::EdgeSE3PointXYZDepth::initialEstimate(), g2o::EdgeSE3PointXYZ::initialEstimate(), g2o::EdgeSE2Offset::initialEstimate(), g2o::deprecated::EdgeSE3PointXYZ::initialEstimate(), g2o::EdgeSE2PointXYOffset::initialEstimate(), g2o::EdgeSE3PointXYZDisparity::initialEstimate(), g2o::deprecated::EdgeSE3Offset::initialEstimate(), g2o::deprecated::EdgeSE3PointXYZDisparity::initialEstimate(), g2o::EdgeSE2::initialEstimate(), g2o::EdgeSE2Line2D::initialEstimate(), g2o::deprecated::EdgeSE3::initialEstimate(), g2o::EdgeSim3::initialEstimate(), TargetOdometry3DEdge::initialEstimate(), main(), Robot::move(), g2o::SparseOptimizerIncremental::optimize(), g2o::SparseOptimizerOnline::optimize(), g2o::Gm2dlIO::readGm2dl(), g2o::VertexCam::setEstimate(), and g2o::SolverSLAM2DLinear::solveOrientation().

101 { _estimate = et; updateCache();}
EstimateType _estimate
Definition: base_vertex.h:106
template<int D, typename T >
double BaseVertex::solveDirect ( double  lambda = 0)
virtual

updates the current vertex with the direct solution x += H_ii

Returns
the determinant of the inverted hessian

Implements g2o::OptimizableGraph::Vertex.

Definition at line 37 of file base_vertex.h.

Referenced by g2o::BaseVertex< 6, Vector6d >::bData().

40  {
41 
template<int D, typename T>
virtual int g2o::BaseVertex< D, T >::stackSize ( ) const
inlinevirtual

return the stack size

Implements g2o::OptimizableGraph::Vertex.

Definition at line 96 of file base_vertex.h.

96 {return _backup.size();}
BackupStackType _backup
Definition: base_vertex.h:107

Member Data Documentation

template<int D, typename T>
Eigen::Matrix<double, D, 1, Eigen::ColMajor> g2o::BaseVertex< D, T >::_b
protected
template<int D, typename T>
const int g2o::BaseVertex< D, T >::Dimension = D
static

dimension of the estimate (minimal) in the manifold space

Definition at line 57 of file base_vertex.h.

Referenced by g2o::BaseVertex< 6, Vector6d >::copyB().


The documentation for this class was generated from the following files: