Classes | Public Types | Private Attributes | List of all members
gtsam::Pose3 Class Reference

#include <Pose3.h>

Inheritance diagram for gtsam::Pose3:
Inheritance graph
[legend]

Classes

struct  ChartAtOrigin
 

Public Types

typedef Rot3 Rotation
 
typedef Point3 Translation
 
- Public Types inherited from gtsam::LieGroup< Pose3, 6 >
enum  
 
typedef OptionalJacobian< N, NChartJacobian
 
typedef Eigen::Matrix< double, N, NJacobian
 
typedef Eigen::Matrix< double, N, 1 > TangentVector
 

Private Attributes

Rot3 R_
 Rotation gRp, between global and pose frame. More...
 
Point3 t_
 Translation gPp, from global origin to pose frame origin. More...
 

Standard Constructors

 Pose3 ()
 
 Pose3 (const Pose3 &pose)
 
 Pose3 (const Rot3 &R, const Point3 &t)
 
 Pose3 (const Pose2 &pose2)
 
 Pose3 (const Matrix &T)
 
static Pose3 Create (const Rot3 &R, const Point3 &t, OptionalJacobian< 6, 3 > HR={}, OptionalJacobian< 6, 3 > Ht={})
 Named constructor with derivatives. More...
 
static std::optional< Pose3Align (const Point3Pairs &abPointPairs)
 
static std::optional< Pose3Align (const Matrix &a, const Matrix &b)
 

Testable

void print (const std::string &s="") const
 print with optional string More...
 
bool equals (const Pose3 &pose, double tol=1e-9) const
 assert equality up to a tolerance More...
 

Group

Pose3 inverse () const
 inverse transformation with derivatives More...
 
Pose3 operator* (const Pose3 &T) const
 compose syntactic sugar More...
 
Pose3 interpolateRt (const Pose3 &T, double t) const
 
static Pose3 Identity ()
 identity for group operation More...
 

Lie Group

Matrix6 AdjointMap () const
 
Vector6 Adjoint (const Vector6 &xi_b, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_xib={}) const
 
Vector6 AdjointTranspose (const Vector6 &x, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_x={}) const
 The dual version of Adjoint. More...
 
static Pose3 Expmap (const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
 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={})
 Log map at identity - return the canonical coordinates $ [R_x,R_y,R_z,T_x,T_y,T_z] $ of this rotation. More...
 
static Matrix6 adjointMap (const Vector6 &xi)
 
static Vector6 adjoint (const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi={}, OptionalJacobian< 6, 6 > H_y={})
 
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={}, OptionalJacobian< 6, 6 > H_y={})
 
static Matrix6 ExpmapDerivative (const Vector6 &xi)
 Derivative of Expmap. More...
 
static Matrix6 LogmapDerivative (const Pose3 &xi)
 Derivative of Logmap. More...
 
static Matrix3 ComputeQforExpmapDerivative (const Vector6 &xi, double nearZeroThreshold=1e-5)
 
static Matrix wedge (double wx, double wy, double wz, double vx, double vy, double vz)
 

Group Action on Point3

Point3 transformFrom (const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
 takes point in Pose coordinates and transforms it to world coordinates More...
 
Matrix transformFrom (const Matrix &points) const
 transform many points in Pose coordinates and transform to world. More...
 
Point3 operator* (const Point3 &point) const
 
Point3 transformTo (const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
 takes point in world coordinates and transforms it to Pose coordinates More...
 
Matrix transformTo (const Matrix &points) const
 transform many points in world coordinates and transform to Pose. More...
 

Standard Interface

const Rot3rotation (OptionalJacobian< 3, 6 > Hself={}) const
 get rotation More...
 
const Point3translation (OptionalJacobian< 3, 6 > Hself={}) const
 get translation More...
 
double x () const
 get x More...
 
double y () const
 get y More...
 
double z () const
 get z More...
 
Matrix4 matrix () const
 
Pose3 transformPoseFrom (const Pose3 &aTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HaTb={}) const
 
Pose3 transformPoseTo (const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HwTb={}) const
 
double range (const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
 
double range (const Pose3 &pose, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 6 > Hpose={}) const
 
Unit3 bearing (const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
 
Unit3 bearing (const Pose3 &pose, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 6 > Hpose={}) const
 

Advanced Interface

Pose3 slerp (double t, const Pose3 &other, OptionalJacobian< 6, 6 > Hx={}, OptionalJacobian< 6, 6 > Hy={}) const
 Spherical Linear interpolation between *this and other. More...
 
static std::pair< size_t, size_ttranslationInterval ()
 
static std::pair< size_t, size_trotationInterval ()
 
GTSAM_EXPORT friend std::ostream & operator<< (std::ostream &os, const Pose3 &p)
 Output stream operator. More...
 

Additional Inherited Members

- Public Member Functions inherited from gtsam::LieGroup< Pose3, 6 >
Pose3 between (const Pose3 &g) const
 
Pose3 between (const Pose3 &g, ChartJacobian H1, ChartJacobian H2={}) const
 
SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
GTSAM_EXPORT SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
Pose3 compose (const Pose3 &g) const
 
Pose3 compose (const Pose3 &g, ChartJacobian H1, ChartJacobian H2={}) const
 
SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
GTSAM_EXPORT SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
const Pose3derived () const
 
Pose3 expmap (const TangentVector &v) const
 
Pose3 expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
 expmap with optional derivatives More...
 
Pose3 inverse (ChartJacobian H) const
 
TangentVector localCoordinates (const Pose3 &g) const
 localCoordinates as required by manifold concept: finds tangent vector between *this and g More...
 
TangentVector localCoordinates (const Pose3 &g, ChartJacobian H1, ChartJacobian H2={}) const
 localCoordinates with optional derivatives More...
 
TangentVector logmap (const Pose3 &g) const
 
TangentVector logmap (const Pose3 &g, ChartJacobian H1, ChartJacobian H2={}) const
 logmap with optional derivatives More...
 
Pose3 retract (const TangentVector &v) const
 retract as required by manifold concept: applies v at *this More...
 
Pose3 retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
 retract with optional derivatives More...
 
- Static Public Member Functions inherited from gtsam::LieGroup< Pose3, 6 >
static TangentVector LocalCoordinates (const Pose3 &g)
 LocalCoordinates at origin: possible in Lie group because it has an identity. More...
 
static TangentVector LocalCoordinates (const Pose3 &g, ChartJacobian H)
 LocalCoordinates at origin with optional derivative. More...
 
static Pose3 Retract (const TangentVector &v)
 Retract at origin: possible in Lie group because it has an identity. More...
 
static Pose3 Retract (const TangentVector &v, ChartJacobian H)
 Retract at origin with optional derivative. More...
 

Detailed Description

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

Definition at line 37 of file Pose3.h.

Member Typedef Documentation

◆ Rotation

Pose Concept requirements

Definition at line 41 of file Pose3.h.

◆ Translation

Definition at line 42 of file Pose3.h.

Constructor & Destructor Documentation

◆ Pose3() [1/5]

gtsam::Pose3::Pose3 ( )
inline

Default constructor is origin

Definition at line 55 of file Pose3.h.

◆ Pose3() [2/5]

gtsam::Pose3::Pose3 ( const Pose3 pose)
inline

Copy constructor

Definition at line 58 of file Pose3.h.

◆ Pose3() [3/5]

gtsam::Pose3::Pose3 ( const Rot3 R,
const Point3 t 
)
inline

Construct from R,t

Definition at line 63 of file Pose3.h.

◆ Pose3() [4/5]

Pose3::Pose3 ( const Pose2 pose2)
explicit

Construct from Pose2

instantiate concept checks

Definition at line 35 of file Pose3.cpp.

◆ Pose3() [5/5]

gtsam::Pose3::Pose3 ( const Matrix T)
inline

Constructor from 4*4 matrix

Definition at line 71 of file Pose3.h.

Member Function Documentation

◆ adjoint()

Vector6 Pose3::adjoint ( const Vector6 &  xi,
const Vector6 &  y,
OptionalJacobian< 6, 6 >  Hxi = {},
OptionalJacobian< 6, 6 >  H_y = {} 
)
static

Action of the adjointMap on a Lie-algebra vector y, with optional derivatives

Definition at line 116 of file Pose3.cpp.

◆ Adjoint()

Vector6 Pose3::Adjoint ( const Vector6 &  xi_b,
OptionalJacobian< 6, 6 >  H_this = {},
OptionalJacobian< 6, 6 >  H_xib = {} 
) 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()

Definition at line 67 of file Pose3.cpp.

◆ adjoint_()

static Vector6 gtsam::Pose3::adjoint_ ( const Vector6 &  xi,
const Vector6 &  y 
)
inlinestatic

Definition at line 191 of file Pose3.h.

◆ AdjointMap()

Matrix6 Pose3::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)

Definition at line 57 of file Pose3.cpp.

◆ adjointMap()

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

Definition at line 106 of file Pose3.cpp.

◆ adjointMap_()

static Matrix6 gtsam::Pose3::adjointMap_ ( const Vector6 &  xi)
inlinestatic

Definition at line 190 of file Pose3.h.

◆ AdjointTranspose()

Vector6 Pose3::AdjointTranspose ( const Vector6 &  x,
OptionalJacobian< 6, 6 >  H_this = {},
OptionalJacobian< 6, 6 >  H_x = {} 
) const

The dual version of Adjoint.

Definition at line 85 of file Pose3.cpp.

◆ adjointTranspose()

Vector6 Pose3::adjointTranspose ( const Vector6 &  xi,
const Vector6 &  y,
OptionalJacobian< 6, 6 >  Hxi = {},
OptionalJacobian< 6, 6 >  H_y = {} 
)
static

The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.

Definition at line 134 of file Pose3.cpp.

◆ Align() [1/2]

std::optional< Pose3 > Pose3::Align ( const Matrix a,
const Matrix b 
)
static

Definition at line 475 of file Pose3.cpp.

◆ Align() [2/2]

std::optional< Pose3 > Pose3::Align ( const Point3Pairs abPointPairs)
static

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.

Definition at line 452 of file Pose3.cpp.

◆ bearing() [1/2]

Unit3 Pose3::bearing ( const Point3 point,
OptionalJacobian< 2, 6 >  Hself = {},
OptionalJacobian< 2, 3 >  Hpoint = {} 
) const

Calculate bearing to a landmark

Parameters
point3D location of landmark
Returns
bearing (Unit3)

Definition at line 425 of file Pose3.cpp.

◆ bearing() [2/2]

Unit3 Pose3::bearing ( const Pose3 pose,
OptionalJacobian< 2, 6 >  Hself = {},
OptionalJacobian< 2, 6 >  Hpose = {} 
) const

Calculate bearing to another pose

Parameters
other3D location and orientation of other body. The orientation information is ignored.
Returns
bearing (Unit3)

Definition at line 442 of file Pose3.cpp.

◆ ComputeQforExpmapDerivative()

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

Definition at line 243 of file Pose3.cpp.

◆ Create()

Pose3 Pose3::Create ( const Rot3 R,
const Point3 t,
OptionalJacobian< 6, 3 >  HR = {},
OptionalJacobian< 6, 3 >  Ht = {} 
)
static

Named constructor with derivatives.

Definition at line 41 of file Pose3.cpp.

◆ equals()

bool Pose3::equals ( const Pose3 pose,
double  tol = 1e-9 
) const

assert equality up to a tolerance

Definition at line 157 of file Pose3.cpp.

◆ Expmap()

Pose3 Pose3::Expmap ( const Vector6 &  xi,
OptionalJacobian< 6, 6 >  Hxi = {} 
)
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?)

Definition at line 169 of file Pose3.cpp.

◆ ExpmapDerivative()

Matrix6 Pose3::ExpmapDerivative ( const Vector6 &  xi)
static

Derivative of Expmap.

Definition at line 286 of file Pose3.cpp.

◆ Identity()

static Pose3 gtsam::Pose3::Identity ( )
inlinestatic

identity for group operation

Definition at line 106 of file Pose3.h.

◆ interpolateRt()

Pose3 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
TEnd point of interpolation.
tA value in [0, 1].

Definition at line 162 of file Pose3.cpp.

◆ inverse()

Pose3 Pose3::inverse ( ) const

inverse transformation with derivatives

Definition at line 49 of file Pose3.cpp.

◆ Logmap()

Vector6 Pose3::Logmap ( const Pose3 pose,
OptionalJacobian< 6, 6 >  Hpose = {} 
)
static

Log map at identity - return the canonical coordinates $ [R_x,R_y,R_z,T_x,T_y,T_z] $ of this rotation.

Definition at line 188 of file Pose3.cpp.

◆ LogmapDerivative()

Matrix6 Pose3::LogmapDerivative ( const Pose3 xi)
static

Derivative of Logmap.

Definition at line 296 of file Pose3.cpp.

◆ matrix()

Matrix4 Pose3::matrix ( ) const

convert to 4*4 matrix

Definition at line 323 of file Pose3.cpp.

◆ operator*() [1/2]

Point3 gtsam::Pose3::operator* ( const Point3 point) const
inline

syntactic sugar for transformFrom

Definition at line 260 of file Pose3.h.

◆ operator*() [2/2]

Pose3 gtsam::Pose3::operator* ( const Pose3 T) const
inline

compose syntactic sugar

Definition at line 114 of file Pose3.h.

◆ print()

void Pose3::print ( const std::string &  s = "") const

print with optional string

Definition at line 152 of file Pose3.cpp.

◆ range() [1/2]

double Pose3::range ( const Point3 point,
OptionalJacobian< 1, 6 >  Hself = {},
OptionalJacobian< 1, 3 >  Hpoint = {} 
) const

Calculate range to a landmark

Parameters
point3D location of landmark
Returns
range (double)

Definition at line 399 of file Pose3.cpp.

◆ range() [2/2]

double Pose3::range ( const Pose3 pose,
OptionalJacobian< 1, 6 >  Hself = {},
OptionalJacobian< 1, 6 >  Hpose = {} 
) const

Calculate range to another pose

Parameters
poseOther SO(3) pose
Returns
range (double)

Definition at line 416 of file Pose3.cpp.

◆ rotation()

const Rot3 & Pose3::rotation ( OptionalJacobian< 3, 6 >  Hself = {}) const

get rotation

Definition at line 315 of file Pose3.cpp.

◆ 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

Definition at line 375 of file Pose3.h.

◆ slerp()

Pose3 Pose3::slerp ( double  t,
const Pose3 other,
OptionalJacobian< 6, 6 >  Hx = {},
OptionalJacobian< 6, 6 >  Hy = {} 
) const

Spherical Linear interpolation between *this and other.

Parameters
sa value between 0 and 1.5
otherfinal point of interpolation geodesic on manifold

Definition at line 488 of file Pose3.cpp.

◆ transformFrom() [1/2]

Matrix Pose3::transformFrom ( const Matrix points) const

transform many points in Pose coordinates and transform to world.

Parameters
points3*N matrix in Pose coordinates
Returns
points in world coordinates, as 3*N Matrix

Definition at line 362 of file Pose3.cpp.

◆ transformFrom() [2/2]

Point3 Pose3::transformFrom ( const Point3 point,
OptionalJacobian< 3, 6 >  Hself = {},
OptionalJacobian< 3, 3 >  Hpoint = {} 
) const

takes point in Pose coordinates and transforms it to world coordinates

Parameters
pointpoint in Pose coordinates
Hselfoptional 3*6 Jacobian wrpt this pose
Hpointoptional 3*3 Jacobian wrpt point
Returns
point in world coordinates

Definition at line 347 of file Pose3.cpp.

◆ transformPoseFrom()

Pose3 Pose3::transformPoseFrom ( const Pose3 aTb,
OptionalJacobian< 6, 6 >  Hself = {},
OptionalJacobian< 6, 6 >  HaTb = {} 
) 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.

Definition at line 331 of file Pose3.cpp.

◆ transformPoseTo()

Pose3 Pose3::transformPoseTo ( const Pose3 wTb,
OptionalJacobian< 6, 6 >  Hself = {},
OptionalJacobian< 6, 6 >  HwTb = {} 
) const

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

Definition at line 338 of file Pose3.cpp.

◆ transformTo() [1/2]

Matrix Pose3::transformTo ( const Matrix points) const

transform many points in world coordinates and transform to Pose.

Parameters
points3*N matrix in world coordinates
Returns
points in Pose coordinates, as 3*N Matrix

Definition at line 390 of file Pose3.cpp.

◆ transformTo() [2/2]

Point3 Pose3::transformTo ( const Point3 point,
OptionalJacobian< 3, 6 >  Hself = {},
OptionalJacobian< 3, 3 >  Hpoint = {} 
) const

takes point in world coordinates and transforms it to Pose coordinates

Parameters
pointpoint in world coordinates
Hselfoptional 3*6 Jacobian wrpt this pose
Hpointoptional 3*3 Jacobian wrpt point
Returns
point in Pose coordinates

Definition at line 371 of file Pose3.cpp.

◆ translation()

const Point3 & Pose3::translation ( OptionalJacobian< 3, 6 >  Hself = {}) const

get translation

Definition at line 308 of file Pose3.cpp.

◆ 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

Definition at line 366 of file Pose3.h.

◆ wedge()

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

wedge for Pose3:

Parameters
xi6-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

Definition at line 233 of file Pose3.h.

◆ x()

double gtsam::Pose3::x ( ) const
inline

get x

Definition at line 292 of file Pose3.h.

◆ y()

double gtsam::Pose3::y ( ) const
inline

get y

Definition at line 297 of file Pose3.h.

◆ z()

double gtsam::Pose3::z ( ) const
inline

get z

Definition at line 302 of file Pose3.h.

Friends And Related Function Documentation

◆ operator<<

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

Output stream operator.

Definition at line 493 of file Pose3.cpp.

Member Data Documentation

◆ R_

Rot3 gtsam::Pose3::R_
private

Rotation gRp, between global and pose frame.

Definition at line 46 of file Pose3.h.

◆ t_

Point3 gtsam::Pose3::t_
private

Translation gPp, from global origin to pose frame origin.

Definition at line 47 of file Pose3.h.


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


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:52:40