Inheritance diagram for gtsam::Pose3:

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

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

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

static Pose3	Create (const Rot3 &R, const Point3 &t, OptionalJacobian< 6, 3 > HR=boost::none, OptionalJacobian< 6, 3 > Ht=boost::none)
	Named constructor with derivatives.

static boost::optional< Pose3 >	Align (const std::vector< Point3Pair > &abPointPairs)
	Create Pose3 by aligning two point pairs A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point Note this allows for noise on the points but in that case the mapping will not be exact.

Group
Pose3	inverse () const
	inverse transformation with derivatives

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

Pose3	interpolateRt (const Pose3 &T, double t) const
	Interpolate between two poses via individual rotation and translation interpolation. More...

static Pose3	identity ()
	identity for group operation

Lie Group
Matrix6	AdjointMap () const
	Calculate Adjoint map, transforming a twist in 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)

Vector6	Adjoint (const Vector6 &xi_b, OptionalJacobian< 6, 6 > H_this=boost::none, OptionalJacobian< 6, 6 > H_xib=boost::none) const
	Apply this pose's AdjointMap Ad_g to a twist \( \xi_b \), i.e. More...

Vector6	AdjointTranspose (const Vector6 &x, OptionalJacobian< 6, 6 > H_this=boost::none, OptionalJacobian< 6, 6 > H_x=boost::none) const
	The dual version of Adjoint.

static Pose3	Expmap (const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost::none)
	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 &pose, OptionalJacobian< 6, 6 > Hpose=boost::none)
	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 Vector6 &xi)
	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. More...

static Vector6	adjoint (const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi=boost::none, OptionalJacobian< 6, 6 > H_y=boost::none)
	Action of the adjointMap on a Lie-algebra vector y, with optional derivatives.

static Matrix6	adjointMap_ (const Vector6 &xi)

static Vector6	adjoint_ (const Vector6 &xi, const Vector6 &y)

static Vector6	adjointTranspose (const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi=boost::none, OptionalJacobian< 6, 6 > H_y=boost::none)
	The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.

static Matrix6	ExpmapDerivative (const Vector6 &xi)
	Derivative of Expmap.

static Matrix6	LogmapDerivative (const Pose3 &xi)
	Derivative of Logmap.

static Matrix3	ComputeQforExpmapDerivative (const Vector6 &xi, double nearZeroThreshold=1e-5)
	Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix J_r(xi) = [J_(w) Z_3x3; Q_r J_(w)] where J_(w) is the SO3 Expmap right derivative. More...

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

Public Member Functions
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	transformFrom (const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
	takes point in Pose coordinates and transforms it to world coordinates More...

Point3	operator* (const Point3 &point) const
	syntactic sugar for transformFrom

Point3	transformTo (const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
	takes point in world coordinates and transforms it to Pose coordinates More...

Standard Interface
const Rot3 &	rotation (OptionalJacobian< 3, 6 > Hself=boost::none) const
	get rotation

const Point3 &	translation (OptionalJacobian< 3, 6 > Hself=boost::none) 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	transformPoseFrom (const Pose3 &aTb, OptionalJacobian< 6, 6 > Hself=boost::none, OptionalJacobian< 6, 6 > HaTb=boost::none) const
	Assuming self == wTa, takes a pose aTb in local coordinates and transforms it to world coordinates wTb = wTa * aTb. More...

Pose3	transformPoseTo (const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself=boost::none, OptionalJacobian< 6, 6 > HwTb=boost::none) const
	Assuming self == wTa, takes a pose wTb in world coordinates and transforms it to local coordinates aTb = inv(wTa) * wTb.

double	range (const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
	Calculate range to a landmark. More...

double	range (const Pose3 &pose, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 6 > Hpose=boost::none) const
	Calculate range to another pose. More...

Unit3	bearing (const Point3 &point, OptionalJacobian< 2, 6 > Hself=boost::none, OptionalJacobian< 2, 3 > Hpoint=boost::none) const
	Calculate bearing to a landmark. More...

Unit3	bearing (const Pose3 &pose, OptionalJacobian< 2, 6 > Hself=boost::none, OptionalJacobian< 2, 6 > Hpose=boost::none) const
	Calculate bearing to another pose. More...

Public Member Functions inherited from gtsam::LieGroup< Pose3, 6 >
const Pose3 &	derived () const

Pose3	compose (const Pose3 &g) const

Pose3	compose (const Pose3 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const

SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

Pose3	between (const Pose3 &g) const

Pose3	between (const Pose3 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const

SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

Pose3	inverse (ChartJacobian H) const

Pose3	expmap (const TangentVector &v) const
	expmap as required by manifold concept Applies exponential map to v and composes with *this

Pose3	expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
	expmap with optional derivatives

TangentVector	logmap (const Pose3 &g) const
	logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g

TangentVector	logmap (const Pose3 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
	logmap with optional derivatives

Pose3	retract (const TangentVector &v) const
	retract as required by manifold concept: applies v at *this

Pose3	retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
	retract with optional derivatives

TangentVector	localCoordinates (const Pose3 &g) const
	localCoordinates as required by manifold concept: finds tangent vector between *this and g

TangentVector	localCoordinates (const Pose3 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
	localCoordinates with optional derivatives

Public Types
typedef Rot3	Rotation
	Pose Concept requirements.

typedef Point3	Translation

Public Types inherited from gtsam::LieGroup< Pose3, 6 >
enum

typedef OptionalJacobian< N, N >	ChartJacobian

typedef Eigen::Matrix< double, N, N >	Jacobian

typedef Eigen::Matrix< double, N, 1 >	TangentVector

Classes
struct	ChartAtOrigin

Additional Inherited Members
Static Public Member Functions inherited from gtsam::LieGroup< Pose3, 6 >
static Pose3	Retract (const TangentVector &v)
	Retract at origin: possible in Lie group because it has an identity.

static Pose3	Retract (const TangentVector &v, ChartJacobian H)
	Retract at origin with optional derivative.

static TangentVector	LocalCoordinates (const Pose3 &g)
	LocalCoordinates at origin: possible in Lie group because it has an identity.

static TangentVector	LocalCoordinates (const Pose3 &g, ChartJacobian H)
	LocalCoordinates at origin with optional derivative.

Constructor & Destructor Documentation

◆ Pose3()

gtsam::Pose3::Pose3 ( const Pose2 & pose2 )

explicit

Construct from Pose2.

instantiate concept checks

Member Function Documentation

◆ Adjoint()

Vector6 gtsam::Pose3::Adjoint	(	const Vector6 &	xi_b,
		OptionalJacobian< 6, 6 >	H_this = `boost::none`,
		OptionalJacobian< 6, 6 >	H_xib = `boost::none`
	)		const

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 \) Note that H_xib = AdjointMap()

◆ adjointMap()

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

static

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.

◆ bearing() [1/2]

Unit3 gtsam::Pose3::bearing	(	const Point3 &	point,
		OptionalJacobian< 2, 6 >	Hself = `boost::none`,
		OptionalJacobian< 2, 3 >	Hpoint = `boost::none`
	)		const

Calculate bearing to a landmark.

Parameters

point 3D location of landmark

Returns: bearing (Unit3)

◆ bearing() [2/2]

Unit3 gtsam::Pose3::bearing	(	const Pose3 &	pose,
		OptionalJacobian< 2, 6 >	Hself = `boost::none`,
		OptionalJacobian< 2, 6 >	Hpose = `boost::none`
	)		const

Calculate bearing to another pose.

Parameters

other 3D location and orientation of other body. The orientation information is ignored.

Returns: bearing (Unit3)

◆ ComputeQforExpmapDerivative()

Matrix3 gtsam::Pose3::ComputeQforExpmapDerivative	(	const Vector6 &	xi,
		double	nearZeroThreshold = `1e-5`
	)

static

Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix J_r(xi) = [J_(w) Z_3x3; Q_r J_(w)] where J_(w) is the SO3 Expmap right derivative.

(see Chirikjian11book2, pg 44, eq 10.95. The closed-form formula is identical to formula 102 in Barfoot14tro where Q_l of the SE3 Expmap left derivative matrix is given.

◆ Expmap()

Pose3 gtsam::Pose3::Expmap	(	const Vector6 &	xi,
		OptionalJacobian< 6, 6 >	Hxi = `boost::none`
	)

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?)

◆ interpolateRt()

Pose3 gtsam::Pose3::interpolateRt	(	const Pose3 &	T,
		double	t
	)		const

inline

Interpolate between two poses via individual rotation and translation interpolation.

The default "interpolate" method defined in Lie.h minimizes the geodesic distance on the manifold, leading to a screw motion interpolation in Cartesian space, which might not be what is expected. In contrast, this method executes a straight line interpolation for the translation, while still using interpolate (aka "slerp") for the rotational component. This might be more intuitive in many applications.

Parameters

T	End point of interpolation.
t	A value in [0, 1].

◆ range() [1/2]

double gtsam::Pose3::range	(	const Point3 &	point,
		OptionalJacobian< 1, 6 >	Hself = `boost::none`,
		OptionalJacobian< 1, 3 >	Hpoint = `boost::none`
	)		const

Calculate range to a landmark.

Parameters

point 3D location of landmark

Returns: range (double)

◆ range() [2/2]

double gtsam::Pose3::range	(	const Pose3 &	pose,
		OptionalJacobian< 1, 6 >	Hself = `boost::none`,
		OptionalJacobian< 1, 6 >	Hpose = `boost::none`
	)		const

Calculate range to another pose.

Parameters

pose	Other SO(3) pose

Returns: range (double)

◆ rotationInterval()

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

◆ transformFrom()

Point3 gtsam::Pose3::transformFrom	(	const Point3 &	point,
		OptionalJacobian< 3, 6 >	Hself = `boost::none`,
		OptionalJacobian< 3, 3 >	Hpoint = `boost::none`
	)		const

takes point in Pose coordinates and transforms it to world coordinates

Parameters

point	point in Pose coordinates
Hself	optional 3*6 Jacobian wrpt this pose
Hpoint	optional 3*3 Jacobian wrpt point

Returns: point in world coordinates

◆ transformPoseFrom()

Pose3 gtsam::Pose3::transformPoseFrom	(	const Pose3 &	aTb,
		OptionalJacobian< 6, 6 >	Hself = `boost::none`,
		OptionalJacobian< 6, 6 >	HaTb = `boost::none`
	)		const

Assuming self == wTa, takes a pose aTb in local coordinates and transforms it to world coordinates wTb = wTa * aTb.

This is identical to compose.

◆ transformTo()

Point3 gtsam::Pose3::transformTo	(	const Point3 &	point,
		OptionalJacobian< 3, 6 >	Hself = `boost::none`,
		OptionalJacobian< 3, 3 >	Hpoint = `boost::none`
	)		const

takes point in world coordinates and transforms it to Pose coordinates

Parameters

point	point in world coordinates
Hself	optional 3*6 Jacobian wrpt this pose
Hpoint	optional 3*3 Jacobian wrpt point

Returns: point in Pose coordinates

◆ translationInterval()

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

◆ wedge()

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:

/Users/dellaert/git/github/gtsam/geometry/Pose3.h
/Users/dellaert/git/github/gtsam/geometry/Pose3.cpp

Advanced Interface

Standard Constructors

Group

Lie Group

Public Member Functions

Public Types

Classes

Additional Inherited Members

Constructor & Destructor Documentation

◆ Pose3()

Member Function Documentation

◆ Adjoint()

◆ adjointMap()

◆ bearing() [1/2]

◆ bearing() [2/2]

◆ ComputeQforExpmapDerivative()

◆ Expmap()

◆ interpolateRt()

◆ range() [1/2]

◆ range() [2/2]

◆ rotationInterval()

◆ transformFrom()

◆ transformPoseFrom()

◆ transformTo()

◆ translationInterval()

◆ wedge()