g2o::VertexSE3 Class Reference

3D pose Vertex, represented as an Isometry3 More...

#include <vertex_se3.h>

Inheritance diagram for g2o::VertexSE3:

Collaboration diagram for g2o::VertexSE3:

Public Member Functions
	VertexSE3 ()
virtual void	setToOriginImpl ()
	sets the node to the origin (used in the multilevel stuff)
virtual bool	read (std::istream &is)
	read the vertex from a stream, i.e., the internal state of the vertex
virtual bool	write (std::ostream &os) const
	write the vertex to a stream
virtual bool	setEstimateDataImpl (const double *est)
virtual bool	getEstimateData (double *est) const
virtual int	estimateDimension () const
virtual bool	setMinimalEstimateDataImpl (const double *est)
virtual bool	getMinimalEstimateData (double *est) const
virtual int	minimalEstimateDimension () const
virtual void	oplusImpl (const double *update)
SE3Quat	G2O_ATTRIBUTE_DEPRECATED (estimateAsSE3Quat() const)
	wrapper function to use the old SE3 type
void	G2O_ATTRIBUTE_DEPRECATED (setEstimateFromSE3Quat(const SE3Quat &se3))
	wrapper function to use the old SE3 type
Public Member Functions inherited from g2o::BaseVertex< 6, Isometry3 >
	BaseVertex ()
BaseVertex &	operator= (const BaseVertex &)=delete
virtual const double &	hessian (int i, int j) const
	get the element from the hessian matrix
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
virtual double *	bData ()
	return a pointer to the b vector associated with this vertex
virtual void	clearQuadraticForm ()
virtual double	solveDirect (double lambda=0)
HessianBlockType &	A ()
	return the hessian block associated with the vertex
virtual void	push ()
	backup the position of the vertex to a stack
virtual void	pop ()
virtual void	discardTop ()
virtual int	stackSize () const
	return the stack size
const EstimateType &	estimate () const
	return the current estimate of the vertex
void	setEstimate (const EstimateType &et)
	set the estimate for the vertex also calls updateCache()
Public Member Functions inherited from g2o::OptimizableGraph::Vertex
	Vertex ()
virtual	~Vertex ()
void	setToOrigin ()
	sets the node to the origin (used in the multilevel stuff)
virtual double &	hessian (int i, int j)=0
virtual double &	b (int i)=0
bool	setEstimateData (const double *estimate)
bool	setEstimateData (const std::vector< double > &estimate)
template<typename Derived>
bool	setEstimateData (const Eigen::MatrixBase< Derived > &estimate)
virtual bool	getEstimateData (std::vector< double > &estimate) const
template<typename Derived>
bool	getEstimateData (Eigen::MatrixBase< Derived > &estimate) const
bool	setMinimalEstimateData (const double *estimate)
bool	setMinimalEstimateData (const std::vector< double > &estimate)
template<typename Derived>
bool	setMinimalEstimateData (const Eigen::MatrixBase< Derived > &estimate)
virtual bool	getMinimalEstimateData (std::vector< double > &estimate) const
template<typename Derived>
bool	getMinimalEstimateData (Eigen::MatrixBase< Derived > &estimate) const
void	oplus (const double *v)
int	hessianIndex () const
int	G2O_ATTRIBUTE_DEPRECATED (tempIndex() const)
void	setHessianIndex (int ti)
	set the temporary index of the vertex in the parameter blocks
void	G2O_ATTRIBUTE_DEPRECATED (setTempIndex(int ti))
bool	fixed () const
	true => this node is fixed during the optimization
void	setFixed (bool fixed)
	true => this node should be considered fixed during the optimization
bool	marginalized () const
	true => this node is marginalized out during the optimization
void	setMarginalized (bool marginalized)
	true => this node should be marginalized out during the optimization
int	dimension () const
	dimension of the estimated state belonging to this node
virtual void	setId (int id)
void	setColInHessian (int c)
	set the row of this vertex in the Hessian
int	colInHessian () const
	get the row of this vertex in the Hessian
const OptimizableGraph *	graph () const
OptimizableGraph *	graph ()
void	lockQuadraticForm ()
void	unlockQuadraticForm ()
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
int	id () const
	returns the id
const EdgeSet &	edges () const
	returns the set of hyper-edges that are leaving/entering in this vertex
EdgeSet &	edges ()
	returns the set of hyper-edges that are leaving/entering in this vertex
virtual HyperGraphElementType	elementType () const
Public Member Functions inherited from g2o::HyperGraph::HyperGraphElement
virtual	~HyperGraphElement ()
Public Member Functions inherited from g2o::HyperGraph::DataContainer
	DataContainer ()
virtual	~DataContainer ()
const Data *	userData () const
	the user data associated with this vertex
Data *	userData ()
void	setUserData (Data *obs)
void	addUserData (Data *obs)

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Static Public Attributes
static const int	orthogonalizeAfter
Static Public Attributes inherited from g2o::BaseVertex< 6, Isometry3 >
static const int	Dimension
	dimension of the estimate (minimal) in the manifold space

Protected Attributes
int	_numOplusCalls
Protected Attributes inherited from g2o::BaseVertex< 6, Isometry3 >
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
Public Types inherited from g2o::BaseVertex< 6, Isometry3 >
using	EstimateType
using	BackupStackType
using	HessianBlockType

Detailed Description

3D pose Vertex, represented as an Isometry3

3D pose vertex, represented as an Isometry3, i.e., an affine transformation which is constructed by only concatenating rotation and translation matrices. Hence, no scaling or projection. To avoid that the rotational part of the Isometry3 gets numerically unstable we compute the nearest orthogonal matrix after a large number of calls to the oplus method.

The parameterization for the increments constructed is a 6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.

Definition at line 50 of file vertex_se3.h.

Constructor & Destructor Documentation

VertexSE3()

g2o::VertexSE3::VertexSE3

(

)

Definition at line 41 of file vertex_se3.cpp.

                     : BaseVertex<6, Isometry3>(), _numOplusCalls(0) {
  setToOriginImpl();
  updateCache();
}

References _numOplusCalls, g2o::BaseVertex< 6, Isometry3 >::BaseVertex(), setToOriginImpl(), and g2o::OptimizableGraph::Vertex::updateCache().

Referenced by g2o::OnlineVertexSE3::OnlineVertexSE3(), and g2o::VertexSCam::VertexSCam().

Member Function Documentation

estimateDimension()

virtual int g2o::VertexSE3::estimateDimension ( ) const

inlinevirtual

returns the dimension of the extended representation used by get/setEstimate(double*) -1 if it is not supported

Reimplemented from g2o::OptimizableGraph::Vertex.

Definition at line 76 of file vertex_se3.h.

{ return 7; }

G2O_ATTRIBUTE_DEPRECATED() [1/2]

SE3Quat g2o::VertexSE3::G2O_ATTRIBUTE_DEPRECATED ( estimateAsSE3Quat() const )

inline

wrapper function to use the old SE3 type

Definition at line 111 of file vertex_se3.h.

                                                              {
    return internal::toSE3Quat(estimate());
  }

References g2o::BaseVertex< 6, Isometry3 >::estimate(), and g2o::internal::toSE3Quat().

G2O_ATTRIBUTE_DEPRECATED() [2/2]

void g2o::VertexSE3::G2O_ATTRIBUTE_DEPRECATED ( setEstimateFromSE3Quat(const SE3Quat &se3) )

inline

wrapper function to use the old SE3 type

Definition at line 115 of file vertex_se3.h.

                                                                            {
    setEstimate(internal::fromSE3Quat(se3));
  }

References g2o::internal::fromSE3Quat(), and g2o::BaseVertex< 6, Isometry3 >::setEstimate().

getEstimateData()

virtual bool g2o::VertexSE3::getEstimateData ( double * estimate ) const

inlinevirtual

writes the estimater to an array of double

Returns

true on success

Reimplemented from g2o::OptimizableGraph::Vertex.

Definition at line 70 of file vertex_se3.h.

                                                  {
    Eigen::Map<Vector7> v(est);
    v = internal::toVectorQT(_estimate);
    return true;
  }

References g2o::BaseVertex< 6, Isometry3 >::_estimate, and g2o::internal::toVectorQT().

getMinimalEstimateData()

virtual bool g2o::VertexSE3::getMinimalEstimateData ( double * estimate ) const

inlinevirtual

writes the estimate to an array of double

Returns

true on success

Reimplemented from g2o::OptimizableGraph::Vertex.

Definition at line 84 of file vertex_se3.h.

                                                         {
    Eigen::Map<Vector6> v(est);
    v = internal::toVectorMQT(_estimate);
    return true;
  }

References g2o::BaseVertex< 6, Isometry3 >::_estimate, and g2o::internal::toVectorMQT().

minimalEstimateDimension()

virtual int g2o::VertexSE3::minimalEstimateDimension ( ) const

inlinevirtual

returns the dimension of the extended representation used by get/setEstimate(double*) -1 if it is not supported

Reimplemented from g2o::OptimizableGraph::Vertex.

Definition at line 90 of file vertex_se3.h.

{ return 6; }

oplusImpl()

virtual void g2o::VertexSE3::oplusImpl ( const double * update )

inlinevirtual

update the position of this vertex. The update is in the form (x,y,z,qx,qy,qz) whereas (x,y,z) represents the translational update and (qx,qy,qz) corresponds to the respective elements. The missing element qw of the quaternion is recovred by || (qw,qx,qy,qz) || == 1 => qw = sqrt(1 - || (qx,qy,qz) ||

Implements g2o::OptimizableGraph::Vertex.

Reimplemented in g2o::OnlineVertexSE3, and g2o::VertexSCam.

Definition at line 99 of file vertex_se3.h.

                                               {
    Eigen::Map<const Vector6> v(update);
    Isometry3 increment = internal::fromVectorMQT(v);
    _estimate = _estimate * increment;
    if (++_numOplusCalls > orthogonalizeAfter) {
      _numOplusCalls = 0;
      internal::approximateNearestOrthogonalMatrix(
          _estimate.matrix().topLeftCorner<3, 3>());
    }
  }

References g2o::BaseVertex< 6, Isometry3 >::_estimate, _numOplusCalls, g2o::internal::approximateNearestOrthogonalMatrix(), g2o::internal::fromVectorMQT(), and orthogonalizeAfter.

Referenced by g2o::OnlineVertexSE3::oplusImpl(), and g2o::VertexSCam::oplusImpl().

read()

bool g2o::VertexSE3::read ( std::istream & is )

virtual

read the vertex from a stream, i.e., the internal state of the vertex

Implements g2o::OptimizableGraph::Vertex.

Reimplemented in g2o::VertexSCam, and g2o::VertexSE3Euler.

Definition at line 46 of file vertex_se3.cpp.

                                   {
  Vector7 est;
  bool state = internal::readVector(is, est);
  setEstimate(internal::fromVectorQT(est));
  return state;
}

References g2o::internal::fromVectorQT(), g2o::internal::readVector(), and g2o::BaseVertex< 6, Isometry3 >::setEstimate().

setEstimateDataImpl()

virtual bool g2o::VertexSE3::setEstimateDataImpl ( const double * )

inlinevirtual

writes the estimater to an array of double

Returns

true on success

Reimplemented from g2o::OptimizableGraph::Vertex.

Definition at line 64 of file vertex_se3.h.

                                                      {
    Eigen::Map<const Vector7> v(est);
    _estimate = internal::fromVectorQT(v);
    return true;
  }

References g2o::BaseVertex< 6, Isometry3 >::_estimate, and g2o::internal::fromVectorQT().

setMinimalEstimateDataImpl()

virtual bool g2o::VertexSE3::setMinimalEstimateDataImpl ( const double * )

inlinevirtual

sets the initial estimate from an array of double

Returns

true on success

Reimplemented from g2o::OptimizableGraph::Vertex.

Definition at line 78 of file vertex_se3.h.

                                                             {
    Eigen::Map<const Vector6> v(est);
    _estimate = internal::fromVectorMQT(v);
    return true;
  }

References g2o::BaseVertex< 6, Isometry3 >::_estimate, and g2o::internal::fromVectorMQT().

setToOriginImpl()

virtual void g2o::VertexSE3::setToOriginImpl ( )

inlinevirtual

sets the node to the origin (used in the multilevel stuff)

Implements g2o::OptimizableGraph::Vertex.

Definition at line 59 of file vertex_se3.h.

{ _estimate = Isometry3::Identity(); }

References g2o::BaseVertex< 6, Isometry3 >::_estimate.

Referenced by VertexSE3().

write()

bool g2o::VertexSE3::write ( std::ostream & os ) const

virtual

write the vertex to a stream

Implements g2o::OptimizableGraph::Vertex.

Reimplemented in g2o::VertexSCam, and g2o::VertexSE3Euler.

Definition at line 53 of file vertex_se3.cpp.

                                          {
  return internal::writeVector(os, internal::toVectorQT(estimate()));
}

References g2o::BaseVertex< 6, Isometry3 >::estimate(), g2o::internal::toVectorQT(), and g2o::internal::writeVector().

Referenced by main().

Member Data Documentation

_numOplusCalls

int g2o::VertexSE3::_numOplusCalls

protected

store how often opluse was called to trigger orthogonaliation of the rotation matrix

Definition at line 120 of file vertex_se3.h.

Referenced by oplusImpl(), and VertexSE3().

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

g2o::VertexSE3::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 52 of file vertex_se3.h.

orthogonalizeAfter

const int g2o::VertexSE3::orthogonalizeAfter

static

Initial value:

=
      1000

Definition at line 54 of file vertex_se3.h.

Referenced by oplusImpl().

---

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

• /build/reproducible-path/g2o-0~20230806/g2o/types/slam3d/vertex_se3.h
• /build/reproducible-path/g2o-0~20230806/g2o/types/slam3d/vertex_se3.cpp