Class CPose3D

Defined in File CPose3D.h

Inheritance Relationships

Base Types

public mrpt::poses::CPose< CPose3D, 6 > (Template Class CPose)
public mrpt::serialization::CSerializable
public mrpt::Stringifyable

Class Documentation

class CPose3D : public mrpt::poses::CPose<CPose3D, 6>, public mrpt::serialization::CSerializable, public mrpt::Stringifyable

SE(3) rigid-body pose with cached rotation matrix — the preferred class for 3D transformations.

Represents an element of SE(3) = SO(3) ⋉ R³. Internally stored as:

A 3-vector m_coords = (x, y, z) — translation in metres.
A 3×3 rotation matrix m_ROT ∈ SO(3) — always kept normalised.

The corresponding 4x4 homogeneous matrix is:

*   T = | R  t |   where R in SO(3), t = (x,y,z)^T
*       | 0  1 |
*

Angle parameterisation (intrinsic ZYX / Tait-Bryan): Yaw/Pitch/Roll angles (yaw=psi, pitch=theta, roll=phi) are derived on demand from m_ROT via getYawPitchRoll():

Rotate about the body Z axis by yaw (psi).
Rotate about the new body Y axis by pitch (theta).
Rotate about the new body X axis by roll (phi).

So R = Rz(yaw) * Ry(pitch) * Rx(roll). This is equivalent to extrinsic rotations about the fixed axes in the reverse order (X then Y then Z). The representation is singular (gimbal lock) when pitch = +-pi/2.

To change pose components, always use setFromValues() — never assign to m_ROT directly using yaw/pitch/roll since the cached angles would become stale. The rotation matrix is always the primary storage; yaw/pitch/roll are recomputed lazily.

Operator semantics (SE(3) group):

a + b (⊕): pose composition — maps the frame of b into the frame of a. In matrix form: T_a · T_b.
a - b (⊖): relative pose — pose of a as seen from b. In matrix form: T_b⁻¹ · T_a.
Unary operator-: SE(3) group inverse. Not elementwise negation of (x,y,z,yaw,pitch,roll).
pose + point: applies the SE(3) transformation to a 3D point (composePoint).

Alternative representations:

As a unit quaternion + translation: CPose3DQuat / getAsQuaternion().
As a Lie algebra twist (se(3) tangent vector): see mrpt::poses::Lie.

Serializable: supports mrpt::serialization::CArchive and YAML schema serialization.

../../output_staging/generated/doxygen/xml/CPose3D.gif

See also

mrpt::math::TPose3D, CPoseOrPoint, CPoint3D, CPose3DQuat, mrpt::math::CQuaternion, mrpt::poses::Lie

Note

See also: “A tutorial on SE(3) transformation parameterizations and

on-manifold optimization”, J.L. Blanco. blanco_se3_tutorial

STL-like methods and typedefs

using value_type = double: The type of the elements

using reference = double&

using const_reference = double

using size_type = std::size_t

using difference_type = std::ptrdiff_t

static constexpr std::size_t static_size = 6

static inline constexpr size_type size()

static inline constexpr bool empty()

static inline constexpr size_type max_size()

static inline void resize(size_t n)

Constructors

CPose3D(): Default constructor, with all the coordinates set to zero.

CPose3D(const double x, const double y, const double z, const double yaw = 0, const double pitch = 0, const double roll = 0)

Constructor with Initialization of the pose, translation (x,y,z) in meters, (yaw,pitch,roll) angles in radians.

See also

FromXYZYawPitchRoll()

explicit CPose3D(const math::CMatrixDouble &m): Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than or equal 3x4, since only those first values are used (the last row of a homogeneous 4x4 matrix are always fixed).

explicit CPose3D(const math::CMatrixDouble44 &m): Constructor from a 4x4 homogeneous matrix:

template<class MATRIX33, class VECTOR3> inline CPose3D(const MATRIX33 &rot, const VECTOR3 &xyz): Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a mrpt::math::TPoint3D

inline CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CVectorFixedDouble<3> &xyz): This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

explicit CPose3D(const CPose2D&): Constructor from a CPose2D object.

explicit CPose3D(const CPoint3D&): Constructor from a CPoint3D object.

explicit CPose3D(const mrpt::math::TPose3D&): Constructor from lightweight object.

mrpt::math::TPose3D asTPose() const

CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z): Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement.

explicit CPose3D(const CPose3DQuat&): Constructor from a CPose3DQuat.

inline CPose3D(TConstructorFlags_Poses): Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument

inline explicit CPose3D(const mrpt::math::CVectorFixedDouble<12> &vec12)

Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose

See also

setFrom12Vector, getAs12Vector

static inline CPose3D Identity(): Returns the identity transformation

static inline CPose3D FromXYZYawPitchRoll(double x, double y, double z, double yaw, double pitch, double roll): Builds a pose from a translation (x,y,z) in meters and (yaw,pitch,roll) angles in radians.

Note

(New in MRPT 2.1.8)

static inline CPose3D FromYawPitchRoll(double yaw, double pitch, double roll): Builds a pose with a null translation and (yaw,pitch,roll) angles in radians.

Note

(New in MRPT 2.1.8)

static inline CPose3D FromTranslation(double x, double y, double z): Builds a pose with a translation without rotation

Note

(New in MRPT 2.1.8)

static inline CPose3D FromTranslation(const mrpt::math::TPoint3D &t)

template<class MATRIX> static inline CPose3D FromHomogeneousMatrix(const MATRIX &m): Builds a pose with a 4x4 homogeneous matrix

Note

(New in MRPT 2.1.8)

template<class MATRIX, class VECTOR> static inline CPose3D FromRotationAndTranslation(const MATRIX &rot, const VECTOR &t): Builds a pose with a 3x3 SO(3) rotation matrix and a translation vector.

Note

(New in MRPT 2.1.8)

static inline CPose3D FromQuaternion(const mrpt::math::CQuaternionDouble &q): Builds a pose from a quaternion (and no translation).

Note

(New in MRPT 2.1.8)

static inline CPose3D FromQuaternionAndTranslation(const mrpt::math::CQuaternionDouble &q, double x, double y, double z): Builds a pose from a quaternion and a (x,y,z) translation.

Note

(New in MRPT 2.1.8)

template<typename Point3DLike> static inline CPose3D FromQuaternionAndTranslation(const mrpt::math::CQuaternionDouble &q, const Point3DLike &pt): Builds a pose from a quaternion and a 3D translation.

Note

(New in MRPT 2.3.3)

Access SO(3), SE(3), R(3)

void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const

Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).

inline void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const

Get the 3x3 rotation matrix

See also

getHomogeneousMatrix

inline const mrpt::math::CMatrixDouble33 &getRotationMatrix() const: This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)

Sets the 3x3 rotation matrix

See also

getRotationMatrix, getHomogeneousMatrix

inline const mrpt::math::TPoint3D translation() const: Returns the (x,y,z) translational part of the SE(3) transformation.

Pose-pose and pose-point compositions and operators

inline CPose3D operator+(const CPose3D &b) const

SE(3) group composition: ret = this ⊕ b.

If this is the pose of frame A in world W, and b is the pose of frame B in A, then ret is the pose of B in W. In matrix form: T_this · T_b.

CPoint3D operator+(const CPoint3D &b) const: Applies the SE(3) transformation to a CPoint3D: ret = this ⊕ b.

CPoint3D operator+(const CPoint2D &b) const: Applies the SE(3) transformation to a CPoint2D (z=0): ret = this ⊕ b.

void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const: Computes the spherical (range, yaw, pitch) coordinates of a 3D point expressed in the global frame, as observed from the sensor frame defined by this pose. The angles follow the same ZY intrinsic convention as CPose3D.

void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint = std::nullopt, mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose = std::nullopt, mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3 = std::nullopt, bool use_small_rot_approx = false) const

Transforms a 3D point from the local frame of this pose into the global frame. Computes: g = R * l + t

Optional Jacobians (all w.r.t. the input quantities):

out_jacobian_df_dpoint (3x3): dg/dl = R.
out_jacobian_df_dpose (3x6): dg/d(t, yaw, pitch, roll) in the yaw/pitch/roll parameterisation. Set use_small_rot_approx=true for the first-order linearisation (valid only for small rotations).
out_jacobian_df_dse3 (3x6): dg/dxi where xi is the 6D locally-Euclidean tangent-space vector of SE(3) (see blanco_se3_tutorial).

See also

inverseComposePoint, composePoint(const TPoint3D&)

inline void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint3D &global_point) const: An alternative, slightly more efficient way of doing G = P (+) L with G and L being 3D points and P this 6D pose.

Note

local_point is passed by value to allow global and local point to be the same variable

inline mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D &l) const

inline void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint2D &global_point) const: This version of the method assumes that the resulting point has no Z component (use with caution!)

inline void composePoint(double lx, double ly, double lz, float &gx, float &gy, float &gz) const: An alternative, slightly more efficient way of doing G = P (+) L with G and L being 3D points and P this 6D pose.

mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const: Rotates a vector (i.e. like composePoint(), but ignoring translation)

mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D &global) const: Inverse of rotateVector(), i.e. using the inverse rotation matrix

void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref<mrpt::math::CMatrixDouble33> out_jacobian_df_dpoint = std::nullopt, mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dpose = std::nullopt, mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3 = std::nullopt) const

Transforms a 3D point from the global frame into the local frame of this pose. Computes: l = R^T * (g - t)

Optional Jacobians follow the same conventions as composePoint():

out_jacobian_df_dpoint (3x3): dl/dg = R^T.
out_jacobian_df_dpose (3×6): w.r.t. yaw/pitch/roll parameterisation.
out_jacobian_df_dse3 (3×6): w.r.t. SE(3) tangent-space vector ξ (see blanco_se3_tutorial).

See also

composePoint, composeFrom

inline void inverseComposePoint(const mrpt::math::TPoint3D &g, mrpt::math::TPoint3D &l) const: This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

inline mrpt::math::TPoint3D inverseComposePoint(const mrpt::math::TPoint3D &g) const

inline void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps = 1e-6) const

overload for 2D points

Throws:: If – the z component of the result is greater than some epsilon

void composeFrom(const CPose3D &A, const CPose3D &B): In-place SE(3) group composition: computes this = A ⊕ B (matrix form: T_A · T_B). Slightly more efficient than this = A + B as it avoids a temporary.

Note

A or B may safely alias this.

inline CPose3D &operator+=(const CPose3D &b): Compound-assignment composition: this = this ⊕ b. b may safely alias this.

void inverseComposeFrom(const CPose3D &A, const CPose3D &B)

In-place relative pose: computes this = A ⊖ B = B⁻¹ ⊕ A (matrix form: T_B⁻¹ · T_A). Slightly more efficient than this = A - B as it avoids a temporary.

See also

composeFrom, composePoint

Note

A or B may safely alias this.

inline CPose3D operator-(const CPose3D &b) const: SE(3) relative pose: ret = this ⊖ b = b⁻¹ ⊕ this. Returns the pose of this expressed in the frame of b.

void inverse()

Replaces this pose with its SE(3) group inverse in-place. If T = [R|t], then T⁻¹ = [Rᵀ | -Rᵀ·t].

See also

operator-

Note

Not the same as negating all 6 components individually.

inline void changeCoordinatesReference(const CPose3D &p): makes: this = p (+) this

Access and modify contents

void addComponents(const CPose3D &p)

Scalar sum of all 6 components: This is different from poses composition, which is implemented as “+” operators.

See also

normalizeAngles

void normalizeAngles()

Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)

See also

addComponents

void operator*=(const double s): Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).

void setFromValues(const double x0, const double y0, const double z0, const double yaw = 0, const double pitch = 0, const double roll = 0)

Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.

See also

getYawPitchRoll, setYawPitchRoll

template<typename VECTORLIKE> inline void setFromXYZQ(const VECTORLIKE &v, size_t index_offset = 0)

Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-element vector.

inline void setYawPitchRoll(const double yaw_, const double pitch_, const double roll_)

Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix.

See also

getYawPitchRoll, setFromValues

template<class ARRAYORVECTOR> inline void setFrom12Vector(const ARRAYORVECTOR &vec12)

Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose

See also

getAs12Vector

template<class ARRAYORVECTOR> inline void getAs12Vector(ARRAYORVECTOR &vec12) const

Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose

See also

setFrom12Vector

void getYawPitchRoll(double &yaw, double &pitch, double &roll) const

Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.

See also

setFromValues, yaw, pitch, roll

inline double yaw() const

Get the YAW angle (in radians)

See also

setFromValues

inline double pitch() const

Get the PITCH angle (in radians)

See also

setFromValues

inline double roll() const

Get the ROLL angle (in radians)

See also

setFromValues

void asVector(vector_t &v) const: Returns a 6x1 vector with [x y z yaw pitch roll]’

void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref<mrpt::math::CMatrixDouble43> out_dq_dr = std::nullopt) const

Converts the SO(3) rotation to a unit quaternion q = (qr, qx, qy, qz).

The translation (x,y,z) is ignored — only m_ROT is converted.

The mapping from intrinsic ZYX angles (yaw, pitch, roll) to the Hamilton unit quaternion (scalar-first) is:

*   Let cy=cos(yaw/2), sy=sin(yaw/2)
*       cp=cos(pitch/2), sp=sin(pitch/2)
*       cr=cos(roll/2),  sr=sin(roll/2)
*
*   q = [ cr*cp*cy + sr*sp*sy ]   (qr, real/scalar part)
*       [ sr*cp*cy - cr*sp*sy ]   (qx)
*       [ cr*sp*cy + sr*cp*sy ]   (qy)
*       [ cr*cp*sy - sr*sp*cy ]   (qz)
*

Parameters:

q – [out] The resulting unit quaternion in (qr, qx, qy, qz) / Hamilton / scalar-first order.
out_dq_dr – [out] If provided, the 4x3 Jacobian dq/d(yaw,pitch,roll) is computed and stored here.

mrpt::math::CMatrixDouble33 jacobian_rodrigues_from_YPR() const

mrpt::math::CMatrixDouble66 jacobian_pose_rodrigues_from_YPR() const

inline double operator[](unsigned int i) const

inline virtual std::string asString() const override

Returns a human-readable textual representation of the object (eg: “[x y

z yaw pitch roll]”, angles in degrees.)

See also

fromString

void fromString(const std::string &s)

Set the current object value from a string generated by ‘asString’ (eg: “[x y z yaw pitch roll]”, angles in deg. )

See also

asString

Throws:: std::exception – On invalid format

void fromStringRaw(const std::string &s): Same as fromString, but without requiring the square brackets in the string

bool isHorizontal(const double tolerance = 0) const: Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards), with a given tolerance (if set to 0 exact horizontality is tested).

double distanceEuclidean6D(const CPose3D &o) const: The euclidean distance between two poses taken as two 6-length vectors (angles in radians).

static inline CPose3D FromString(const std::string &s)

Public Types

enum [anonymous]

Values:

enumerator is_3D_val

enum [anonymous]

Values:

enumerator rotation_dimensions

enum [anonymous]

Values:

enumerator is_PDF_val

using type_value = CPose3D : Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses

Public Functions

CPose3D getOppositeScalar() const: Return the opposite of the current pose instance by taking the negative of all its components individually

inline const type_value &getPoseMean() const

inline type_value &getPoseMean()

Public Members

mrpt::math::CVectorFixedDouble<3> m_coords: The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.

Public Static Functions

static inline constexpr bool is_3D()

static inline constexpr bool is_PDF()

Protected Functions

void rebuildRotationMatrix(): Rebuild the homog matrix from the angles.

inline void updateYawPitchRoll() const: Updates Yaw/pitch/roll members from the m_ROT

Protected Attributes

mrpt::math::CMatrixDouble33 m_ROT: The SO(3) rotation matrix — primary storage for orientation. Access via getRotationMatrix() / setRotationMatrix(); do not modify directly, as that would leave the cached yaw/pitch/roll angles inconsistent.

mutable bool m_ypr_uptodate = {false}: Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.

mutable double m_yaw = {0}: These variables are updated every time that the object rotation matrix is modified (construction, loading from values, pose composition, etc )

double m_pitch = {0}

double m_roll = {0}