Class CPose3DQuat

Nested Relationships

Nested Types

Inheritance Relationships

Base Types

  • public mrpt::poses::CPose< CPose3DQuat, 7 > (Template Class CPose)

  • public mrpt::serialization::CSerializable

  • public mrpt::Stringifyable

Class Documentation

class CPose3DQuat : public mrpt::poses::CPose<CPose3DQuat, 7>, public mrpt::serialization::CSerializable, public mrpt::Stringifyable

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

STL-like methods and typedefs

using value_type = double

The type of the elements

using reference = double&
using const_reference = double
using size_type = std::size_t
using difference_type = std::ptrdiff_t
using iterator = iterator_impl<false>
using const_iterator = iterator_impl<true>
using reverse_iterator = std::reverse_iterator<iterator>
using const_reverse_iterator = std::reverse_iterator<const_iterator>
static constexpr std::size_t static_size = 7
inline void assign(size_t N, const double val)
inline iterator begin()
inline iterator end()
inline const_iterator begin() const
inline const_iterator end() const
inline const_iterator cbegin() const
inline const_iterator cend() const
inline reverse_iterator rbegin()
inline const_reverse_iterator rbegin() const
inline reverse_iterator rend()
inline const_reverse_iterator rend() const
inline const_reverse_iterator crbegin() const
inline const_reverse_iterator crend() const
inline void swap(CPose3DQuat &o)
static inline constexpr size_type size()
static inline constexpr bool empty()
static inline constexpr size_type max_size()
static inline void resize(size_t n)

Public Types

enum [anonymous]

Values:

enumerator is_3D_val
enum [anonymous]

Values:

enumerator rotation_dimensions
enum [anonymous]

Values:

enumerator is_PDF_val
using type_value = CPose3DQuat

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

Public Functions

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

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

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

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

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

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

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

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

inline CPose3DQuat()

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

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

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

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

explicit CPose3DQuat(const CPose3D &p)

Constructor from a CPose3D

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

Constructor from lightweight object.

mrpt::math::TPose3DQuat asTPose() const
explicit CPose3DQuat(const mrpt::math::CMatrixDouble44 &M)

Constructor from a 4x4 homogeneous transformation matrix.

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]’

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.

See also

inverseComposeFrom, composePoint

Note

A or B can be “this” without problems.

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.

See also

composeFrom, composePoint

Note

A or B can be “this” without problems.

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

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

See also

composePoint, composeFrom

template<class POINT1, class POINT2>
inline 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>
inline void inverseComposePoint(const POINT1 &G, POINT2 &L) const

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

See also

inverseComposePoint

inline CPoint3D operator+(const CPoint3D &L) const

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

See also

composePoint

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

inline CPose3DQuat &operator+=(const CPose3DQuat &b)

Make \( this = this \oplus b \)

inline CPose3DQuat operator+(const CPose3DQuat &p) const

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

inline CPose3DQuat &operator-=(const CPose3DQuat &b)

Make \( this = this \ominus b \)

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

inline virtual std::string asString() const override

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]”)

See also

asString

Throws:

std::exception – On invalid format

void fromStringRaw(const std::string &s)

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

inline double operator[](const std::size_t i) const

Read-only [] operator

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

inline const type_value &getPoseMean() const
inline type_value &getPoseMean()

Public Members

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

Public Static Functions

static inline CPose3DQuat FromString(const std::string &s)
static inline constexpr bool is_3D()
static inline constexpr bool is_PDF()
template<bool IsConst>
class iterator_impl

Unified iterator implementation using template parameterization for const-correctness. This eliminates code duplication between iterator and const_iterator.

Public Types

using difference_type = std::ptrdiff_t
using value_type = double
using pointer = std::conditional_t<IsConst, const double*, double*>
using reference = std::conditional_t<IsConst, const double&, double&>
using iterator_category = std::random_access_iterator_tag

Public Functions

iterator_impl() = default
inline iterator_impl(PoseType &obj, difference_type start_idx)
template<bool WasConst, typename = std::enable_if_t<IsConst && !WasConst>>
inline iterator_impl(const iterator_impl<WasConst> &other)
inline reference operator*() const
inline reference operator[](difference_type off) const
inline iterator_impl &operator++()
inline iterator_impl operator++(int)
inline iterator_impl &operator--()
inline iterator_impl operator--(int)
inline iterator_impl &operator+=(difference_type off)
inline iterator_impl operator+(difference_type off) const
inline iterator_impl &operator-=(difference_type off)
inline iterator_impl operator-(difference_type off) const
inline difference_type operator-(const iterator_impl &it) const
inline bool operator==(const iterator_impl &it) const
inline bool operator!=(const iterator_impl &it) const
inline bool operator<(const iterator_impl &it) const
inline bool operator>(const iterator_impl &it) const
inline bool operator<=(const iterator_impl &it) const
inline bool operator>=(const iterator_impl &it) const

Friends

friend class iterator_impl