Inheritance diagram for gtsam::Pose3:

Group
Pose3	inverse (boost::optional< Matrix & > H1=boost::none) const
	inverse transformation with derivatives

Pose3	compose (const Pose3 &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
	compose this transformation onto another (first *this and then p2)

Pose3	operator* (const Pose3 &T) const
	compose syntactic sugar

Pose3	between (const Pose3 &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
	Return relative pose between p1 and p2, in p1 coordinate frame as well as optionally the derivatives.

static Pose3	identity ()
	identity for group operation

Manifold
enum	CoordinatesMode { EXPMAP, FIRST_ORDER }
	Enum to indicate which method should be used in Pose3::retract() and Pose3::localCoordinates() More...

size_t	dim () const
	Dimensionality of the tangent space = 6 DOF.

Pose3	retractFirstOrder (const Vector &d) const
	Retraction from R^6 \( [R_x,R_y,R_z,T_x,T_y,T_z] \) from R^ with fast first-order approximation to the exponential map.

Pose3	retract (const Vector &d, Pose3::CoordinatesMode mode=POSE3_DEFAULT_COORDINATES_MODE) const
	Retraction from R^6 \( [R_x,R_y,R_z,T_x,T_y,T_z] \) to Pose3 manifold neighborhood around current pose.

Vector6	localCoordinates (const Pose3 &T2, Pose3::CoordinatesMode mode=POSE3_DEFAULT_COORDINATES_MODE) const
	Local 6D coordinates \( [R_x,R_y,R_z,T_x,T_y,T_z] \) of Pose3 manifold neighborhood around current pose.

static size_t	Dim ()
	Dimensionality of tangent space = 6 DOF - used to autodetect sizes.

Lie Group
Matrix6	AdjointMap () const
	Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame Ad_pose is 6*6 matrix that when applied to twist xi \( [R_x,R_y,R_z,T_x,T_y,T_z] \), returns Ad_pose(xi)

Vector	Adjoint (const Vector &xi_b) const
	FIXME Not tested - marked as incorrect. More...

static Pose3	Expmap (const Vector &xi)
	Exponential map at identity - create a rotation from canonical coordinates \( [R_x,R_y,R_z,T_x,T_y,T_z] \). More...

static Vector6	Logmap (const Pose3 &p)
	Log map at identity - return the canonical coordinates \( [R_x,R_y,R_z,T_x,T_y,T_z] \) of this rotation.

static Matrix6	adjointMap (const Vector &xi)
	FIXME Not tested - marked as incorrect. More...

static Vector	adjoint (const Vector &xi, const Vector &y, boost::optional< Matrix & > H=boost::none)
	Action of the adjointMap on a Lie-algebra vector y, with optional derivatives.

static Vector	adjointTranspose (const Vector &xi, const Vector &y, boost::optional< Matrix & > H=boost::none)
	The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.

static Matrix6	dExpInv_exp (const Vector &xi)
	Compute the inverse right-trivialized tangent (derivative) map of the exponential map, as detailed in [Kobilarov09siggraph] eq. More...

static Matrix	wedge (double wx, double wy, double wz, double vx, double vy, double vz)
	wedge for Pose3: More...

Advanced Interface
class	boost::serialization::access
	Serialization function.

GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const Pose3 &p)
	Output stream operator.

static std::pair< size_t, size_t >	translationInterval ()
	Return the start and end indices (inclusive) of the translation component of the exponential map parameterization. More...

static std::pair< size_t, size_t >	rotationInterval ()
	Return the start and end indices (inclusive) of the rotation component of the exponential map parameterization. More...

Public Member Functions
Standard Constructors
	Pose3 ()
	Default constructor is origin.

	Pose3 (const Pose3 &pose)
	Copy constructor.

	Pose3 (const Rot3 &R, const Point3 &t)
	Construct from R,t.

	Pose3 (const Pose2 &pose2)
	Construct from Pose2.

	Pose3 (const Matrix &T)
	Constructor from 4*4 matrix.

Testable
void	print (const std::string &s="") const
	print with optional string

bool	equals (const Pose3 &pose, double tol=1e-9) const
	assert equality up to a tolerance

Group Action on Point3
Point3	transform_from (const Point3 &p, boost::optional< Matrix & > Dpose=boost::none, boost::optional< Matrix & > Dpoint=boost::none) const
	takes point in Pose coordinates and transforms it to world coordinates More...

Point3	operator* (const Point3 &p) const
	syntactic sugar for transform_from

Point3	transform_to (const Point3 &p, boost::optional< Matrix & > Dpose=boost::none, boost::optional< Matrix & > Dpoint=boost::none) const
	takes point in world coordinates and transforms it to Pose coordinates More...

Standard Interface
const Rot3 &	rotation () const
	get rotation

const Point3 &	translation () const
	get translation

double	x () const
	get x

double	y () const
	get y

double	z () const
	get z

Matrix4	matrix () const
	convert to 4*4 matrix

Pose3	transform_to (const Pose3 &pose) const
	receives a pose in world coordinates and transforms it to local coordinates

double	range (const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
	Calculate range to a landmark. More...

double	range (const Pose3 &pose, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
	Calculate range to another pose. More...

Public Member Functions inherited from gtsam::DerivedValue< Pose3 >
virtual Value *	clone_ () const
	Create a duplicate object returned as a pointer to the generic Value interface. More...

virtual void	deallocate_ () const
	Destroy and deallocate this object, only if it was originally allocated using clone_().

virtual boost::shared_ptr< Value >	clone () const
	Clone this value (normal clone on the heap, delete with 'delete' operator)

virtual bool	equals_ (const Value &p, double tol=1e-9) const
	equals implementing generic Value interface

virtual Value *	retract_ (const Vector &delta) const
	Generic Value interface version of retract.

virtual Vector	localCoordinates_ (const Value &value2) const
	Generic Value interface version of localCoordinates.

virtual Value &	operator= (const Value &rhs)
	Assignment operator.

	operator const Pose3 & () const
	Conversion to the derived class.

	operator Pose3 & ()
	Conversion to the derived class.

Public Member Functions inherited from gtsam::Value
virtual	~Value ()
	Virutal destructor.

Static Public Attributes
static const size_t	dimension = 6

Public Types
typedef Rot3	Rotation
	Pose Concept requirements.

typedef Point3	Translation

Additional Inherited Members
Protected Member Functions inherited from gtsam::DerivedValue< Pose3 >
DerivedValue< Pose3 > &	operator= (const DerivedValue< Pose3 > &rhs)
	Assignment operator, protected because only the Value or DERIVED assignment operators should be used. More...

Member Enumeration Documentation

enum gtsam::Pose3::CoordinatesMode

Enum to indicate which method should be used in Pose3::retract() and Pose3::localCoordinates()

Enumerator
EXPMAP	The correct exponential map, computationally expensive.
FIRST_ORDER	A fast first-order approximation to the exponential map.

Member Function Documentation

Vector gtsam::Pose3::Adjoint ( const Vector & xi_b ) const

inline

FIXME Not tested - marked as incorrect.

Apply this pose's AdjointMap Ad_g to a twist \( \xi_b \), i.e. a body-fixed velocity, transforming it to the spatial frame \( \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \)

Matrix6 gtsam::Pose3::adjointMap ( const Vector & xi )

static

FIXME Not tested - marked as incorrect.

Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 [ad(w,v)] = [w^, zero3; v^, w^] Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3.

Let \( \hat{\xi}_i \) be the se3 Lie algebra, and \( \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\) be its vector representation. We have the following relationship: \( [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \)

We use this to compute the discrete version of the inverse right-trivialized tangent map, and its inverse transpose in the discrete Euler Poincare' (DEP) operator.

Matrix6 gtsam::Pose3::dExpInv_exp ( const Vector & xi )

static

Compute the inverse right-trivialized tangent (derivative) map of the exponential map, as detailed in [Kobilarov09siggraph] eq.

(15) The full formula is documented in [Celledoni99cmame] Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421� 438, 2003. and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 Ernst Hairer, et al., Geometric Numerical Integration, Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.

Pose3 gtsam::Pose3::Expmap ( const Vector & xi )

static

Exponential map at identity - create a rotation from canonical coordinates \( [R_x,R_y,R_z,T_x,T_y,T_z] \).

Modified from Murray94book version (which assumes w and v normalized?)

double gtsam::Pose3::range	(	const Point3 &	point,
		boost::optional< Matrix & >	H1 = `boost::none`,
		boost::optional< Matrix & >	H2 = `boost::none`
	)		const

Calculate range to a landmark.

Parameters

point 3D location of landmark

Returns: range (double)

double gtsam::Pose3::range	(	const Pose3 &	pose,
		boost::optional< Matrix & >	H1 = `boost::none`,
		boost::optional< Matrix & >	H2 = `boost::none`
	)		const

Calculate range to another pose.

Parameters

pose	Other SO(3) pose

Returns: range (double)

static std::pair<size_t, size_t> gtsam::Pose3::rotationInterval ( )

inlinestatic

Return the start and end indices (inclusive) of the rotation component of the exponential map parameterization.

Returns: a pair of [start, end] indices into the tangent space vector

Point3 gtsam::Pose3::transform_from	(	const Point3 &	p,
		boost::optional< Matrix & >	Dpose = `boost::none`,
		boost::optional< Matrix & >	Dpoint = `boost::none`
	)		const

takes point in Pose coordinates and transforms it to world coordinates

Parameters

p	point in Pose coordinates
Dpose	optional 3*6 Jacobian wrpt this pose
Dpoint	optional 3*3 Jacobian wrpt point

Returns: point in world coordinates

Point3 gtsam::Pose3::transform_to	(	const Point3 &	p,
		boost::optional< Matrix & >	Dpose = `boost::none`,
		boost::optional< Matrix & >	Dpoint = `boost::none`
	)		const

takes point in world coordinates and transforms it to Pose coordinates

Parameters

p	point in world coordinates
Dpose	optional 3*6 Jacobian wrpt this pose
Dpoint	optional 3*3 Jacobian wrpt point

Returns: point in Pose coordinates

static std::pair<size_t, size_t> gtsam::Pose3::translationInterval ( )

inlinestatic

Return the start and end indices (inclusive) of the translation component of the exponential map parameterization.

Returns: a pair of [start, end] indices into the tangent space vector

static Matrix gtsam::Pose3::wedge	(	double	wx,
		double	wy,
		double	wz,
		double	vx,
		double	vy,
		double	vz
	)

inlinestatic

wedge for Pose3:

Parameters

xi	6-dim twist (omega,v) where omega = (wx,wy,wz) 3D angular velocity v (vx,vy,vz) = 3D velocity

Returns: xihat, 4*4 element of Lie algebra that can be exponentiated

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

/home/cbeall3/git/gtsam/gtsam/geometry/Pose3.h
/home/cbeall3/git/gtsam/gtsam/geometry/Pose3.cpp

Group

Manifold

Lie Group

Advanced Interface

Public Member Functions

Static Public Attributes

Public Types

Additional Inherited Members

Member Enumeration Documentation

Member Function Documentation