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

#include <vertex_se3.h>

Inheritance diagram for g2o::VertexSE3:

[legend]

Collaboration diagram for g2o::VertexSE3:

[legend]

Public Member Functions
	VertexSE3 ()

virtual void	setToOriginImpl ()
	sets the node to the origin (used in the multilevel stuff) More...

virtual bool	read (std::istream &is)
	read the vertex from a stream, i.e., the internal state of the vertex More...

virtual bool	write (std::ostream &os) const
	write the vertex to a stream More...

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 More...

void	G2O_ATTRIBUTE_DEPRECATED (setEstimateFromSE3Quat(const SE3Quat &se3))
	wrapper function to use the old SE3 type More...

Public Member Functions inherited from g2o::BaseVertex< 6, Isometry3D >
	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)

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

virtual double *	bData ()
	return a pointer to the b vector associated with this vertex More...

virtual void	clearQuadraticForm ()

virtual double	solveDirect (double lambda=0)

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 (std::vector< double > &estimate) const

bool	setMinimalEstimateData (const double *estimate)

bool	setMinimalEstimateData (const std::vector< double > &estimate)

virtual bool	getMinimalEstimateData (std::vector< double > &estimate) 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 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)

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Static Public Attributes
static const int	orthogonalizeAfter = 1000

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

Protected Attributes
int	_numOplusCalls
	store how often opluse was called to trigger orthogonaliation of the rotation matrix More...

Protected Attributes inherited from g2o::BaseVertex< 6, Isometry3D >
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, Isometry3D >
typedef Isometry3D	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

Detailed Description

3D pose Vertex, represented as an Isometry3D

3D pose vertex, represented as an Isometry3D, 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 Isometry3D 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

g2o::VertexSE3::VertexSE3 ( )

Definition at line 41 of file vertex_se3.cpp.

References setToOriginImpl(), and g2o::OptimizableGraph::Vertex::updateCache().

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

Member Function Documentation

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 78 of file vertex_se3.h.

                                             {
         return 7;
       }

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

inline

wrapper function to use the old SE3 type

Definition at line 117 of file vertex_se3.h.

References g2o::internal::toSE3Quat().

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

g2o::internal::toSE3Quat

SE3Quat toSE3Quat(const Isometry3D &t)

Definition: isometry3d_mappings.cpp:138

g2o::BaseVertex< 6, Isometry3D >::estimate

const EstimateType & estimate() const

return the current estimate of the vertex

Definition: base_vertex.h:99

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

inline

wrapper function to use the old SE3 type

Definition at line 119 of file vertex_se3.h.

References g2o::internal::fromSE3Quat().

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

g2o::BaseVertex< 6, Isometry3D >::setEstimate

void setEstimate(const EstimateType &et)

set the estimate for the vertex also calls updateCache()

Definition: base_vertex.h:101

g2o::internal::fromSE3Quat

Isometry3D fromSE3Quat(const SE3Quat &t)

Definition: isometry3d_mappings.cpp:144

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 72 of file vertex_se3.h.

References g2o::internal::toVectorQT().

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

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 88 of file vertex_se3.h.

References g2o::internal::toVectorMQT().

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

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 94 of file vertex_se3.h.

                                                    {
         return 6;
       }

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::VertexSCam, and g2o::OnlineVertexSE3.

Definition at line 105 of file vertex_se3.h.

References g2o::internal::approximateNearestOrthogonalMatrix(), and g2o::internal::fromVectorMQT().

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

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

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 49 of file vertex_se3.cpp.

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

   {
     Vector7d est;
     for (int i=0; i<7; i++)
       is  >> est[i];
     setEstimate(internal::fromVectorQT(est));
     return true;
   }

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 66 of file vertex_se3.h.

References g2o::internal::fromVectorQT().

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

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 82 of file vertex_se3.h.

References g2o::internal::fromVectorMQT().

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

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.

Referenced by VertexSE3().

                                      {
         _estimate = Isometry3D::Identity();
       }

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 58 of file vertex_se3.cpp.

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

Referenced by main().

   {
     Vector7d est=internal::toVectorQT(_estimate);
     for (int i=0; i<7; i++)
       os << est[i] << " ";
     return os.good();
   }

Member Data Documentation

int g2o::VertexSE3::_numOplusCalls

protected

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

Definition at line 122 of file vertex_se3.h.

g2o::VertexSE3::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 53 of file vertex_se3.h.

const int g2o::VertexSE3::orthogonalizeAfter = 1000

static

Definition at line 55 of file vertex_se3.h.

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

/home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d/vertex_se3.h
/home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d/vertex_se3.cpp

Public Member Functions

Public Attributes

Static Public Attributes

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation