g2o
|
Templatized BaseVertex. More...
#include <base_vertex.h>
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 |
HessianBlockType & | A () |
return the hessian block associated with the vertex More... | |
const HessianBlockType & | A () 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 EstimateType & | estimate () 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 Vertex * | clone () 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 OptimizableGraph * | graph () const |
OptimizableGraph * | graph () |
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 () |
CacheContainer * | cacheContainer () |
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 EdgeSet & | edges () const |
returns the set of hyper-edges that are leaving/entering in this vertex More... | |
EdgeSet & | edges () |
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 () |
HyperGraphElement * | clone () const |
Public Member Functions inherited from g2o::HyperGraph::DataContainer | |
DataContainer () | |
virtual | ~DataContainer () |
const Data * | userData () const |
the user data associated with this vertex More... | |
Data * | userData () |
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 *) |
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.
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.
typedef T g2o::BaseVertex< D, T >::EstimateType |
Definition at line 52 of file base_vertex.h.
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.
BaseVertex::BaseVertex | ( | ) |
Definition at line 29 of file base_vertex.h.
|
inline |
|
inline |
Definition at line 91 of file base_vertex.h.
|
inlinevirtual |
get the b vector element
Implements g2o::OptimizableGraph::Vertex.
Definition at line 76 of file base_vertex.h.
|
inlinevirtual |
|
inline |
return right hand side b of the constructed linear system
Definition at line 87 of file base_vertex.h.
|
inline |
Definition at line 88 of file base_vertex.h.
|
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.
|
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().
|
inlinevirtual |
copies the b vector in the array b_
Implements g2o::OptimizableGraph::Vertex.
Definition at line 71 of file base_vertex.h.
|
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.
|
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().
|
inlinevirtual |
get the element from the hessian matrix
Implements g2o::OptimizableGraph::Vertex.
Definition at line 64 of file base_vertex.h.
|
inlinevirtual |
|
inlinevirtual |
|
inlinevirtual |
|
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().
|
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.
|
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().
|
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().
|
virtual |
updates the current vertex with the direct solution x += H_ii
Implements g2o::OptimizableGraph::Vertex.
Definition at line 37 of file base_vertex.h.
Referenced by g2o::BaseVertex< 6, Vector6d >::bData().
|
inlinevirtual |
return the stack size
Implements g2o::OptimizableGraph::Vertex.
Definition at line 96 of file base_vertex.h.
|
protected |
Definition at line 105 of file base_vertex.h.
Referenced by g2o::BaseVertex< 6, Vector6d >::b(), g2o::BaseVertex< 6, Vector6d >::bData(), and g2o::BaseVertex< 6, Vector6d >::copyB().
|
protected |
Definition at line 107 of file base_vertex.h.
Referenced by g2o::BaseVertex< 6, Vector6d >::discardTop(), g2o::BaseVertex< 6, Vector6d >::pop(), g2o::BaseVertex< 6, Vector6d >::push(), and g2o::BaseVertex< 6, Vector6d >::stackSize().
|
protected |
Definition at line 106 of file base_vertex.h.
Referenced by g2o::BaseVertex< 6, Vector6d >::estimate(), g2o::BaseVertex< 6, Vector6d >::pop(), g2o::BaseVertex< 6, Vector6d >::push(), and g2o::BaseVertex< 6, Vector6d >::setEstimate().
|
protected |
Definition at line 104 of file base_vertex.h.
Referenced by g2o::BaseVertex< 6, Vector6d >::A(), g2o::BaseVertex< 6, Vector6d >::hessian(), g2o::BaseVertex< 6, Vector6d >::hessianData(), and g2o::BaseVertex< 6, Vector6d >::hessianDeterminant().
|
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().