gtsam  3.2.0
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
gtsam::Pose3 Class Reference
+ 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 Rot3rotation () const
 get rotation
 
const Point3translation () 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 Valueclone_ () 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< Valueclone () 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 Valueretract_ (const Vector &delta) const
 Generic Value interface version of retract.
 
virtual Vector localCoordinates_ (const Value &value2) const
 Generic Value interface version of localCoordinates.
 
virtual Valueoperator= (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 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
point3D 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
poseOther 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
ppoint in Pose coordinates
Dposeoptional 3*6 Jacobian wrpt this pose
Dpointoptional 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
ppoint in world coordinates
Dposeoptional 3*6 Jacobian wrpt this pose
Dpointoptional 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
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

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