struct mrpt::math::TPose3D

Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).

Allows coordinate access using [] operator.

See also:


#include <mrpt/math/TPose3D.h>

struct TPose3D:
    public mrpt::math::TPoseOrPoint,
    public mrpt::math::internal::ProvideStaticResize
    // enums

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


    static constexpr TPose3D Identity();

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

    static TPose3D FromString(const std::string& s);
    double& operator [] (size_t i);
    constexpr double operator [] (size_t i) const;
    double norm() const;
    void asVector(std::vector<double>& v) const;
    void asString(std::string& s) const;
    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;

    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


    void resize(std::size_t n);


double x {.0}

X,Y,Z, coords.

double yaw {.0}

Yaw coordinate (rotation angle over Z axis).

double pitch {.0}

Pitch coordinate (rotation angle over Y axis).

double roll {.0}

Roll coordinate (rotation angle over X coordinate).


TPose3D(const TPoint2D& p)

Implicit constructor from TPoint2D.

Zeroes all the unprovided information.

See also:


TPose3D(const TPose2D& p)

Implicit constructor from TPose2D.

Gets the yaw from the 2D pose’s phi, zeroing all the unprovided information.

See also:


TPose3D(const TPoint3D& p)

Implicit constructor from TPoint3D.

Zeroes angular information.

See also:


    double _x,
    double _y,
    double _z,
    double _yaw,
    double _pitch,
    double _roll

Constructor from coordinates.


Default fast constructor.

Initializes to zeros.


static constexpr TPose3D Identity()

Returns the identity transformation, T=eye(4)

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

double norm() const

Pose’s spatial coordinates norm.

void asVector(std::vector<double>& v) const

Gets the pose as a vector of doubles.

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:


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

Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)

\[\begin{split}\mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right)\end{split}\]

With : \(\phi = roll\), \(\theta = pitch\) and \(\psi = yaw\).



If provided, the 4x3 Jacobian of the transformation will be computed and stored here. It’s the Jacobian of the transformation from (yaw pitch roll) to (qr qx qy qz).

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

Operator “oplus” pose composition: “ret=this oplus b”.

[Added in MRPT 2.1.5]

See also:


void fromString(const std::string& s)

Set the current object value from a string generated by ‘asString’ (eg: “[0.02 1.04 -0.8]” )



On invalid format

See also: