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

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)

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

Pose’s spatial coordinates norm.

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

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

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:

CPose3D

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