Struct TPose3D

Inheritance Relationships

Base Types

Struct Documentation

struct TPose3D : public mrpt::math::TPoseOrPoint, public mrpt::math::internal::ProvideStaticResize<TPose3D>

Lightweight 3D rigid-body pose — an element of SE(3).

Parameterises the group SE(3) = SO(3) ⋉ R³ as the 6-vector (x, y, z, yaw, pitch, roll):

  • (x, y, z) — translation in metres.

  • (yaw, pitch, roll) — orientation in radians, using the intrinsic ZYX convention (also known as Tait-Bryan angles):

    1. Rotate about the body Z axis by yaw (ψ).

    2. Rotate about the new body Y axis by pitch (θ).

    3. Rotate about the new body X axis by roll (φ).

    Equivalently (same result, reversed order) as extrinsic rotations about the fixed axes X→Y→Z. The resulting SO(3) rotation matrix is R = Rz(yaw)·Ry(pitch)·Rx(roll).

The equivalent 4x4 homogeneous matrix is: 

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

**Composition (operator+)** implements the SE(3) group product: (this ⊕ b) maps a point expressed in frame b into the frame of this.

**Inverse (operator- unary)** computes the SE(3) group inverse T⁻¹.

**Relative pose (operator- binary)**: b - a = a⁻¹ ⊕ b, the pose of b as seen from a.

This is a lightweight type without serialization or cached rotation matrix. Prefer mrpt::poses::CPose3D when the rotation matrix is needed directly or when composition is repeated with the same pose (avoids redundant trig).

Coordinate access: operator[] with index 0→x, 1→y, 2→z, 3→yaw, 4→pitch, 5→roll.

See also

mrpt::poses::CPose3D, TPose2D, TPoint3D

Public Functions

TPose3D(const TPoint2D &p)

Constructor from TPoint2D: copies (x,y), sets z=0, yaw=pitch=roll=0.

See also

TPoint2D

TPose3D(const TPose2D &p)

Constructor from TPose2D: copies (x,y), sets z=0, maps phi→yaw, pitch=roll=0.

See also

TPose2D

explicit TPose3D(const TPoint3D &p)

Constructor from TPoint3D: copies (x,y,z) as translation; yaw=pitch=roll=0 (no rotation).

See also

TPoint3D

inline constexpr TPose3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)

Constructor from coordinates

constexpr TPose3D() = default

Default fast constructor. Initializes to zeros.

inline double &operator[](size_t i)

Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll

inline constexpr double operator[](size_t i) const

Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll

inline mrpt::math::TPoint3D translation() const

Returns the (x,y,z) translational part of the SE(3) transformation.

inline double norm() const

Euclidean norm of the translational part: |(x,y,z)|. Angular coordinates are ignored.

template<typename Vector>
inline void asVector(Vector &v) const

Gets the pose as a vector of doubles: [x y z yaw pitch roll]

Template Parameters:

Vector – It can be std::vector<double>, Eigen::VectorXd, etc.

template<typename Vector>
inline Vector asVector() const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void asString(std::string &s) const

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

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

See also

fromString

inline std::string asString() const
void getAsQuaternion(mrpt::math::CQuaternion<double> &q, mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 3>> out_dq_dr = std::nullopt) const

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

The translation (x,y,z) is ignored — only the yaw/pitch/roll angles are converted.

The mapping from intrinsic ZYX Euler angles (yaw=psi, pitch=theta, roll=phi) to the Hamilton unit quaternion 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)
*

The quaternion is stored in the order **(qr, qx, qy, qz)** (scalar-first / Hamilton convention).

Parameters:

  • q – [out] The resulting unit quaternion.

  • out_dq_dr – [out] If provided, the 4x3 Jacobian dq/d(yaw,pitch,roll) is computed and stored here.

void composePoint(const TPoint3D &l, TPoint3D &g) const

Transforms a point from the local frame of this pose into the global (world) frame. Equivalent to g = R·l + t, where R and t are the rotation matrix and translation of this pose.

See also

inverseComposePoint, composePoint(const TPoint3D&)

Parameters:

  • l – Point in the local frame.

  • g – [out] Point in the global frame.

TPoint3D composePoint(const TPoint3D &l) const
void inverseComposePoint(const TPoint3D &g, TPoint3D &l) const

Transforms a point from the global (world) frame into the local frame of this pose. Equivalent to l = Rᵀ·(g − t).

See also

composePoint

Parameters:

  • g – Point in the global frame.

  • l – [out] Point in the local frame of this pose.

TPoint3D inverseComposePoint(const TPoint3D &g) const
void composePose(const TPose3D &other, TPose3D &result) const

SE(3) group composition: result = this ⊕ other. In matrix form: T_result = T_this · T_other.

inline mrpt::math::TPose3D operator+(const mrpt::math::TPose3D &b) const

SE(3) group composition — “⊕” operator: ret = this ⊕ b.

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

See also

CPose3D

void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const
inline mrpt::math::CMatrixDouble33 getRotationMatrix() const
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
inline mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
inline mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
void fromString(const std::string &s)

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

See also

asString

Throws:

std::exception – On invalid format

Public Members

double x = {.0}

Translation along X (metres).

double y = {.0}

Translation along Y (metres).

double z = {.0}

Translation along Z (metres).

double yaw = {.0}

Yaw angle psi in radians: rotation about the body (intrinsic) Z axis — first rotation. Range: any real value; conventionally wrapped to (-pi, pi].

double pitch = {.0}

Pitch angle theta in radians: rotation about the (new) body Y axis — second rotation. Range: (-pi/2, pi/2) for a non-singular representation (gimbal-lock at +-pi/2).

double roll = {.0}

Roll angle phi in radians: rotation about the (new) body X axis — third rotation. Range: any real value; conventionally wrapped to (-pi, pi].

Public Static Functions

static inline constexpr TPose3D Identity()

Returns the identity transformation, T=eye(4)

static inline TPose3D FromString(const std::string &s)

See fromString() for a description of the expected string format.

template<typename Vector>
static inline TPose3D FromVector(const Vector &v)

Builds from the first 6 elements of a vector-like object: [x y z yaw pitch roll]

Template Parameters:

Vector – It can be std::vector<double>, Eigen::VectorXd, etc.

static void SO3_to_yaw_pitch_roll(const mrpt::math::CMatrixDouble33 &R, double &yaw, double &pitch, double &roll)

Public Static Attributes

static constexpr std::size_t static_size = 6