class mrpt::poses::CPose3DQuat

A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).

For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer to the 2D/3D Geometry tutorial in the wiki.

To access the translation use x(), y() and z(). To access the rotation, use CPose3DQuat::quat().

This class also behaves like a STL container, since it has begin(), end(), iterators, and can be accessed with the [] operator with indices running from 0 to 6 to access the [x y z qr qx qy qz] as if they were a vector. Thus, a CPose3DQuat can be used as a 7-vector anywhere the MRPT math functions expect any kind of vector.

This class and CPose3D are very similar, and they can be converted to the each other automatically via transformation constructors.

See also:

CPose3D (for a class based on a 4x4 matrix instead of a quaternion), mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic version of this class, mrpt::math::CQuaternion, CPoseOrPoint

#include <mrpt/poses/CPose3DQuat.h>

class CPose3DQuat:
    public mrpt::poses::CPose,
    public mrpt::serialization::CSerializable,
    public mrpt::Stringifyable
{
public:
    // typedefs

    typedef double value_type;
    typedef double& reference;
    typedef double const_reference;
    typedef std::size_t size_type;
    typedef std::ptrdiff_t difference_type;
    typedef std::reverse_iterator<iterator> reverse_iterator;
    typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
    typedef CPose3DQuat type_value;

    // enums

    enum
    {
        is_3D_val = 1,
    };

    enum
    {
        rotation_dimensions = 3,
    };

    enum
    {
        is_PDF_val = 1,
    };

    enum
    {
        static_size = 7,
    };

    // structs

    struct const_iterator;
    struct iterator;

    //
fields

    mrpt::math::CVectorFixedDouble<3> m_coords;
    mrpt::math::CQuaternionDouble m_quat;

    // construction

    CPose3DQuat();
    CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions);
    CPose3DQuat(TConstructorFlags_Poses);
    CPose3DQuat(const double x, const double y, const double z, const mrpt::math::CQuaternionDouble& q);
    CPose3DQuat(const CPose3D& p);
    CPose3DQuat(const mrpt::math::TPose3DQuat& p);
    CPose3DQuat(const mrpt::math::CMatrixDouble44& M);

    //
methods

    void assign(
        const size_t N,
        const double val
        );

    iterator begin();
    iterator end();
    const_iterator begin() const;
    const_iterator end() const;
    reverse_iterator rbegin();
    const_reverse_iterator rbegin() const;
    reverse_iterator rend();
    const_reverse_iterator rend() const;
    void swap(CPose3DQuat& o);
    static constexpr size_type size();
    static constexpr bool empty();
    static constexpr size_type max_size();
    static void resize(const size_t n);
    mrpt::math::CQuaternionDouble& quat();
    const mrpt::math::CQuaternionDouble& quat() const;
    mrpt::math::CVectorFixedDouble<3>& xyz();
    const mrpt::math::CVectorFixedDouble<3>& xyz() const;
    mrpt::math::TPose3DQuat asTPose() const;
    void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const;
    void asVector(vector_t& v) const;
    void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B);
    void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B);

    void composePoint(
        const double lx,
        const double ly,
        const double lz,
        double& gx,
        double& gy,
        double& gz,
        mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint = nullptr,
        mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose = nullptr
        ) const;

    void inverseComposePoint(
        const double gx,
        const double gy,
        const double gz,
        double& lx,
        double& ly,
        double& lz,
        mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint = nullptr,
        mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose = nullptr
        ) const;

    template <class POINT1, class POINT2>
    void composePoint(const POINT1& L, POINT2& G) const;

    template <class POINT1, class POINT2>
    void inverseComposePoint(const POINT1& G, POINT2& L) const;

    CPoint3D operator + (const CPoint3D& L) const;
    mrpt::math::TPoint3D operator + (const mrpt::math::TPoint3D& L) const;
    virtual void operator *= (const double s);
    CPose3DQuat& operator += (const CPose3DQuat& b);
    CPose3DQuat operator + (const CPose3DQuat& p) const;
    CPose3DQuat& operator -= (const CPose3DQuat& b);
    CPose3DQuat operator - (const CPose3DQuat& p) const;
    void inverse();
    virtual std::string asString() const;
    void fromString(const std::string& s);
    void fromStringRaw(const std::string& s);
    double operator [] (unsigned int i) const;
    double& operator [] (unsigned int i);

    void sphericalCoordinates(
        const mrpt::math::TPoint3D& point,
        double& out_range,
        double& out_yaw,
        double& out_pitch,
        mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint = nullptr,
        mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose = nullptr
        ) const;

    const type_value& getPoseMean() const;
    type_value& getPoseMean();
    virtual void setToNaN();
    static CPose3DQuat FromString(const std::string& s);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
};

Inherited Members

public:
    //
methods

    double& x();
    double& y();
    void x(const double v);
    void y(const double v);
    void x_incr(const double v);
    void y_incr(const double v);
    const DERIVEDCLASS& derived() const;
    DERIVEDCLASS& derived();

Typedefs

typedef double value_type

The type of the elements.

typedef CPose3DQuat type_value

Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.

Fields

mrpt::math::CVectorFixedDouble<3> m_coords

The translation vector [x,y,z].

mrpt::math::CQuaternionDouble m_quat

The quaternion [0]:w, [1-3]:x,y,z.

Construction

CPose3DQuat()

Default constructor, initialize translation to zeros and quaternion to no rotation.

CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions)

Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use UNINITIALIZED_POSE as argument to this constructor.

CPose3DQuat(TConstructorFlags_Poses)

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

CPose3DQuat(const double x, const double y, const double z, const mrpt::math::CQuaternionDouble& q)

Constructor with initilization of the pose - the quaternion is normalized to make sure it’s unitary.

CPose3DQuat(const CPose3D& p)

Constructor from a CPose3D.

CPose3DQuat(const mrpt::math::TPose3DQuat& p)

Constructor from lightweight object.

CPose3DQuat(const mrpt::math::CMatrixDouble44& M)

Constructor from a 4x4 homogeneous transformation matrix.

Methods

mrpt::math::CQuaternionDouble& quat()

Read/Write access to the quaternion representing the 3D rotation.

const mrpt::math::CQuaternionDouble& quat() const

Read-only access to the quaternion representing the 3D rotation.

mrpt::math::CVectorFixedDouble<3>& xyz()

Read/Write access to the translation vector in R^3.

const mrpt::math::CVectorFixedDouble<3>& xyz() const

Read-only access to the translation vector in R^3.

void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const

Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).

See also:

getInverseHomogeneousMatrix

void asVector(vector_t& v) const

Returns a 7x1 vector with [x y z qr qx qy qz]’.

Returns a 1x7 vector with [x y z qr qx qy qz].

void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B)

Makes \(this = A \oplus B\) this method is slightly more efficient than “this= A + B;” since it avoids the temporary object.

Makes “this = A (+) B”; this method is slightly more efficient than “this= A + B;” since it avoids the temporary object.

A or B can be “this” without problems.

A or B can be “this” without problems.

See also:

inverseComposeFrom, composePoint

void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B)

Makes \(this = A \ominus B\) this method is slightly more efficient than “this= A - B;” since it avoids the temporary object.

A or B can be “this” without problems.

A or B can be “this” without problems.

See also:

composeFrom, composePoint

composeFrom

void composePoint(
    const double lx,
    const double ly,
    const double lz,
    double& gx,
    double& gy,
    double& gz,
    mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint = nullptr,
    mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose = nullptr
    ) const

Computes the 3D point G such as \(G = this \oplus L\).

See also:

composeFrom, inverseComposePoint

inverseComposeFrom

void inverseComposePoint(
    const double gx,
    const double gy,
    const double gz,
    double& lx,
    double& ly,
    double& lz,
    mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint = nullptr,
    mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose = nullptr
    ) const

Computes the 3D point L such as \(L = G \ominus this\).

Computes the 3D point G such as \(L = G \ominus this\).

See also:

composePoint, composeFrom

composeFrom

template <class POINT1, class POINT2>
void composePoint(
    const POINT1& L,
    POINT2& G
    ) const

Computes the 3D point G such as \(G = this \oplus L\).

POINT1 and POINT1 can be anything supporing [0],[1],[2].

See also:

composePoint

template <class POINT1, class POINT2>
void inverseComposePoint(
    const POINT1& G,
    POINT2& L
    ) const

Computes the 3D point L such as \(L = G \ominus this\).

See also:

inverseComposePoint

CPoint3D operator + (const CPoint3D& L) const

Computes the 3D point G such as \(G = this \oplus L\).

See also:

composePoint

mrpt::math::TPoint3D operator + (const mrpt::math::TPoint3D& L) const

Computes the 3D point G such as \(G = this \oplus L\).

See also:

composePoint

virtual void operator *= (const double s)

Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).

CPose3DQuat& operator += (const CPose3DQuat& b)

Make \(this = this \oplus b\).

CPose3DQuat operator + (const CPose3DQuat& p) const

Return the composed pose \(ret = this \oplus p\).

CPose3DQuat& operator -= (const CPose3DQuat& b)

Make \(this = this \ominus b\).

CPose3DQuat operator - (const CPose3DQuat& p) const

Return the composed pose \(ret = this \ominus p\).

void inverse()

Convert this pose into its inverse, saving the result in itself.

See also:

operator-

virtual std::string asString() const

Returns a human-readable textual representation of the object as: "[x y z qw qx qy qz]"

See also:

fromString

void fromString(const std::string& s)

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

Parameters:

std::exception

On invalid format

See also:

asString

void fromStringRaw(const std::string& s)

Same as fromString, but without requiring the square brackets in the string.

double operator [] (unsigned int i) const

Read only [] operator.

double& operator [] (unsigned int i)

Read/write [] operator.

void sphericalCoordinates(
    const mrpt::math::TPoint3D& point,
    double& out_range,
    double& out_yaw,
    double& out_pitch,
    mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint = nullptr,
    mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose = nullptr
    ) const

Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.

For the coordinate system see the top of this page. If the matrix pointers are not nullptr, the Jacobians will be also computed for the range-yaw-pitch variables wrt the passed 3D point and this 7D pose.

virtual void setToNaN()

Set all data fields to quiet NaN.