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:
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:
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\).
Computes the 3D point G such as \(L = G \ominus this\).
See also:
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:
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:
CPoint3D operator + (const CPoint3D& L) const
Computes the 3D point G such as \(G = this \oplus L\).
See also:
mrpt::math::TPoint3D operator + (const mrpt::math::TPoint3D& L) const
Computes the 3D point G such as \(G = this \oplus L\).
See also:
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:
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:
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.