gtsam  4.1.0 gtsam
gtsam::Pose3 Class Reference
Inheritance diagram for gtsam::Pose3:

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

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< Pose3Align (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

static Pose3 identity ()
identity for group operation

## Lie Group

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)

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

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)
FIXME Not tested - marked as incorrect. More...

static Vector6 adjoint (const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi=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)
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...

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

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 Rot3rotation (OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation

const Point3translation (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 Pose3derived () 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

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.

## Member Function Documentation

 Vector6 gtsam::Pose3::Adjoint ( const Vector6 & 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 Vector6 & 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.

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

## ◆ 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 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 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/Gerry/GIT_REPOS/gtsam/gtsam/geometry/Pose3.h
• /Users/Gerry/GIT_REPOS/gtsam/gtsam/geometry/Pose3.cpp