Loading...
Searching...
No Matches
gtsam::Pose3 Class Reference
Geometry

## Detailed Description

A 3D pose (R,t) : (Rot3,Point3).

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.
Pose3 | slerp (double t, const Pose3 &other, OptionalJacobian< 6, 6 > Hx=boost::none, OptionalJacobian< 6, 6 > Hy=boost::none) const
| Spherical Linear interpolation between *this and other.
static std::pair< size_t, size_t > | translationInterval ()
| Return the start and end indices (inclusive) of the translation component of the exponential map parameterization.
static std::pair< size_t, size_t > | rotationInterval ()
| Return the start and end indices (inclusive) of the rotation component of the exponential map parameterization.

## 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< Pose3 > | Align (const Point3Pairs &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.
static boost::optional< Pose3 > | Align (const Matrix &a, const Matrix &b)

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

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.
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.
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] \).
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.
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.
static Matrix | wedge (double wx, double wy, double wz, double vx, double vy, double vz)
| wedge for Pose3:

## 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
Matrix | transformFrom (const Matrix &points) const
| transform many points in Pose coordinates and transform to world.
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
Matrix | transformTo (const Matrix &points) const
| transform many points in world coordinates and transform to Pose.

## 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.
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.
double | range (const Pose3 &pose, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 6 > Hpose=boost::none) const
| Calculate range to another pose.
Unit3 | bearing (const Point3 &point, OptionalJacobian< 2, 6 > Hself=boost::none, OptionalJacobian< 2, 3 > Hpoint=boost::none) const
| Calculate bearing to a landmark.
Unit3 | bearing (const Pose3 &pose, OptionalJacobian< 2, 6 > Hself=boost::none, OptionalJacobian< 2, 6 > Hpose=boost::none) const
| Calculate bearing to another pose.

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

Public Member Functions inherited from gtsam::LieGroup< Pose3, 6 >
SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
const Pose3 & | derived () 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
TangentVector | logmap (const Pose3 &g) const
| logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
Pose3 | retract (const TangentVector &v) const
| retract as required by manifold concept: applies v at *this
TangentVector | localCoordinates (const Pose3 &g) const
| localCoordinates as required by manifold concept: finds tangent vector between *this and g
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 TangentVector | LocalCoordinates (const Pose3 &g)
| LocalCoordinates at origin: possible in Lie group because it has an identity.

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

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

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

inline static

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

## slerp()

Pose3 gtsam::Pose3::slerp | ( | double | t,
|  | const Pose3 & | other,
|  | OptionalJacobian< 6, 6 > | Hx = boost::none,
|  | OptionalJacobian< 6, 6 > | Hy = boost::none ) const

Spherical Linear interpolation between *this and other.

Parameters

  s | a value between 0 and 1.5
  other | final point of interpolation geodesic on manifold

## transformFrom() [1/2]

Matrix gtsam::Pose3::transformFrom | ( | const Matrix & | points | ) | const

transform many points in Pose coordinates and transform to world.

Parameters

  points | 3*N matrix in Pose coordinates

Returns
  points in world coordinates, as 3*N Matrix

## transformFrom() [2/2]

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() [1/2]

Matrix gtsam::Pose3::transformTo | ( | const Matrix & | points | ) | const

transform many points in world coordinates and transform to Pose.

Parameters

  points | 3*N matrix in world coordinates

Returns
  points in Pose coordinates, as 3*N Matrix

## transformTo() [2/2]

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

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

inline static

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

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

inline static

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:

- /build/gtsam-RkN8wG/gtsam-4.2.2+dfsg/gtsam/geometry/Pose3.h
- /build/gtsam-RkN8wG/gtsam-4.2.2+dfsg/gtsam/geometry/Pose3.cpp