class mrpt::poses::CPose3DQuat

Overview

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 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 std::shared_ptr<mrpt::poses ::CPose3DQuat> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPose3DQuat> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPose3DQuat> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPose3DQuat> ConstUniquePtr;
    typedef double value_type;
    typedef double& reference;
    typedef double const_reference;
    typedef std::size_t size_type;
    typedef std::ptrdiff_t difference_type;
    typedef iterator_impl<false> iterator;
    typedef iterator_impl<true> const_iterator;
    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,
    };

    // classes

    template <bool IsConst>
    class iterator_impl;

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPose3DQuat";
    static constexpr std::size_t static_size = 7;
    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

    static constexpr auto getClassName();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    static std::shared_ptr<CObject> CreateObject();

    template <typename... Args>
    static Ptr Create(Args&&... args);

    template <typename Alloc, typename... Args>
    static Ptr CreateAlloc(
        const Alloc& alloc,
        Args&&... args
        );

    template <typename... Args>
    static UniquePtr CreateUnique(Args&&... args);

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual mrpt::rtti::CObject* clone() const;
    static constexpr size_type size();
    static constexpr bool empty();
    static constexpr size_type max_size();
    static void resize(size_t n);

    void assign(
        size_t N,
        const double val
        );

    iterator begin();
    iterator end();
    const_iterator begin() const;
    const_iterator end() const;
    const_iterator cbegin() const;
    const_iterator cend() const;
    reverse_iterator rbegin();
    const_reverse_iterator rbegin() const;
    reverse_iterator rend();
    const_reverse_iterator rend() const;
    const_reverse_iterator crbegin() const;
    const_reverse_iterator crend() const;
    void swap(CPose3DQuat& o);
    static CPose3DQuat FromString(const std::string& s);
    static constexpr bool is_3D();
    static constexpr bool is_PDF();
    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;
    mrpt::math::CMatrixDouble44 getHomogeneousMatrix() 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 [] (const std::size_t i) const;
    double& operator [] (const std::size_t 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();
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;

    // 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);
    CPoseOrPoint& operator = (const CPoseOrPoint&);
    CPoseOrPoint& operator = (CPoseOrPoint&&);
    const DERIVEDCLASS& derived() const;
    DERIVEDCLASS& derived();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();

Typedefs

typedef std::shared_ptr<mrpt::poses ::CPose3DQuat> Ptr

A type for the associated smart pointer.

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 leaves all the quaternion members uninitialized, 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 initialization of the pose - the quaternion is normalized to ensure 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

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual mrpt::rtti::CObject* clone() const

Returns a deep copy (clone) of the object, indepently of its class.

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

Deprecated Use getHomogeneousMatrix() returning by value instead.

See also:

getInverseHomogeneousMatrix

mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const

Returns the corresponding 4x4 homogeneous transformation matrix.

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.

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.

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.

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 POINT2 can be anything supporting [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’ (e.g.

: “[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 [] (const std::size_t i) const

Read-only [] operator.

double& operator [] (const std::size_t 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.