struct mrpt::math::TPose3D

Overview

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

#include <mrpt/math/TPose3D.h>

struct TPose3D:
    public mrpt::math::TPoseOrPoint,
    public mrpt::math::internal::ProvideStaticResize,
    public mrpt::math::internal::ProvidesStringConversion
{
    // fields

    static constexpr std::size_t static_size = 6;
    double x {.0};
    double y {.0};
    double z {.0};
    double yaw {.0};
    double pitch {.0};
    double roll {.0};

    // construction

    TPose3D(const TPoint2D& p);
    TPose3D(const TPose2D& p);
    TPose3D(const TPoint3D& p);
    TPose3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
    TPose3D();

    // methods

    static constexpr TPose3D Identity();
    static TPose3D FromString(const std::string& s);

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

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

    double& operator [] (size_t i);
    constexpr double operator [] (size_t i) const;
    mrpt::math::TPoint3D translation() const;
    double norm() const;

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

    template <typename Vector>
    Vector asVector() const;

    void asString(std::string& s) const;
    void getAsQuaternion(mrpt::math::CQuaternion<double>& q, mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 3>> out_dq_dr = std::nullopt) const;
    void composePoint(const TPoint3D& l, TPoint3D& g) const;
    TPoint3D composePoint(const TPoint3D& l) const;
    void inverseComposePoint(const TPoint3D& g, TPoint3D& l) const;
    TPoint3D inverseComposePoint(const TPoint3D& g) const;
    void composePose(const TPose3D& other, TPose3D& result) const;
    mrpt::math::TPose3D operator + (const mrpt::math::TPose3D& b) const;
    void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const;
    mrpt::math::CMatrixDouble33 getRotationMatrix() const;
    void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const;
    mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const;
    void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const;
    mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const;
    void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44& HG);
    void fromString(const std::string& s);
};

Inherited Members

public:
    // methods

    void resize(std::size_t n);

Fields

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

Construction

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

TPose3D(const TPoint3D& p)

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

See also:

TPoint3D

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

Constructor from coordinates.

TPose3D()

Default fast constructor.

Initializes to zeros.

Methods

static constexpr TPose3D Identity()

Returns the identity transformation, T=eye(4)

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

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

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

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

Parameters:

Vector

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

double& operator [] (size_t i)

Coordinate access using operator[].

Order: x,y,z,yaw,pitch,roll

constexpr double operator [] (size_t i) const

Coordinate access using operator[].

Order: x,y,z,yaw,pitch,roll

mrpt::math::TPoint3D translation() const

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

double norm() const

Euclidean norm of the translational part: |(x,y,z)|.

Angular coordinates are ignored.

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

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

Parameters:

Vector

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

template <typename Vector>
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

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	The resulting unit quaternion.
out_dq_dr	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.

Parameters:

l	Point in the local frame.
g	Point in the global frame.

See also:

inverseComposePoint, composePoint(const TPoint3D&)

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

Parameters:

g	Point in the global frame.
l	Point in the local frame of this pose.

See also:

composePoint

void composePose(const TPose3D& other, TPose3D& result) const

SE(3) group composition: result = this ⊕ other.

In matrix form: T_result = T_this · T_other.

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

)

Parameters:

std::exception

On invalid format

See also:

asString