struct mrpt::math::TPose3D

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

Allows coordinate access using [] operator.

See also:

mrpt::poses::CPose3D

#include <mrpt/math/TPose3D.h>

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

    enum
    {
        static_size = 6,
    };

    //
fields

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

    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}

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

Construction

TPose3D(const TPoint2D& p)

Implicit constructor from TPoint2D.

Zeroes all the unprovided information.

See also:

TPoint2D

TPose3D(const TPose2D& p)

Implicit constructor from TPose2D.

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

See also:

TPose2D

TPose3D(const TPoint3D& p)

Implicit constructor from TPoint3D.

Zeroes angular information.

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)

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:

fromString

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

Parameters:

out_dq_dr

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

void fromString(const std::string& s)

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

Parameters:

std::exception

On invalid format

See also:

asString