MRPT  1.9.9
mrpt::math::TPose3D Struct Reference

Detailed Description

Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).

Allows coordinate access using [] operator.

See also
mrpt::poses::CPose3D
Examples:
comms_nodelets_example/NodeletsTest_impl.cpp.

Definition at line 23 of file TPose3D.h.

#include <mrpt/math/TPose3D.h>

Inheritance diagram for mrpt::math::TPose3D:

Public Types

enum  { static_size = 6 }
 

Public Member Functions

 TPose3D (const TPoint2D &p)
 Implicit constructor from TPoint2D. More...
 
 TPose3D (const TPose2D &p)
 Implicit constructor from TPose2D. More...
 
 TPose3D (const TPoint3D &p)
 Implicit constructor from TPoint3D. More...
 
constexpr TPose3D (double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
 Constructor from coordinates. More...
 
constexpr TPose3D ()=default
 Default fast constructor. More...
 
double & operator[] (size_t i)
 Coordinate access using operator[]. More...
 
constexpr const double & operator[] (size_t i) const
 Coordinate access using operator[]. More...
 
double norm () const
 Pose's spatial coordinates norm. More...
 
void asVector (std::vector< double > &v) const
 Gets the pose as a vector of doubles. More...
 
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.) More...
 
std::string asString () const
 
void getAsQuaternion (mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixed< double, 4, 3 > *out_dq_dr=nullptr) const
 Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)

\[ \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) \]

With : $ \phi = roll $, $ \theta = pitch $ and $ \psi = yaw $. More...

 
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
 
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)
 Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" ) More...
 
constexpr std::size_t rows () const
 
constexpr std::size_t cols () const
 
constexpr std::size_t size () const
 
void resize (std::size_t n)
 throws if attempted to resize to incorrect length More...
 

Static Public Member Functions

static constexpr TPose3D Identity ()
 Returns the identity transformation, T=eye(4) More...
 
static void SO3_to_yaw_pitch_roll (const mrpt::math::CMatrixDouble33 &R, double &yaw, double &pitch, double &roll)
 

Public Attributes

double x {.0}
 X,Y,Z, coords. More...
 
double y {.0}
 
double z {.0}
 
double yaw {.0}
 Yaw coordinate (rotation angle over Z axis). More...
 
double pitch {.0}
 Pitch coordinate (rotation angle over Y axis). More...
 
double roll {.0}
 Roll coordinate (rotation angle over X coordinate). More...
 

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
Enumerator
static_size 

Definition at line 26 of file TPose3D.h.

Constructor & Destructor Documentation

◆ TPose3D() [1/5]

mrpt::math::TPose3D::TPose3D ( const TPoint2D p)

Implicit constructor from TPoint2D.

Zeroes all the unprovided information.

See also
TPoint2D

Definition at line 272 of file TPoseOrPoint.cpp.

◆ TPose3D() [2/5]

mrpt::math::TPose3D::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

Definition at line 276 of file TPoseOrPoint.cpp.

◆ TPose3D() [3/5]

mrpt::math::TPose3D::TPose3D ( const TPoint3D p)

Implicit constructor from TPoint3D.

Zeroes angular information.

See also
TPoint3D

Definition at line 280 of file TPoseOrPoint.cpp.

◆ TPose3D() [4/5]

constexpr mrpt::math::TPose3D::TPose3D ( double  _x,
double  _y,
double  _z,
double  _yaw,
double  _pitch,
double  _roll 
)
inline

Constructor from coordinates.

Definition at line 61 of file TPose3D.h.

◆ TPose3D() [5/5]

constexpr mrpt::math::TPose3D::TPose3D ( )
default

Default fast constructor.

Initializes to zeros.

Referenced by Identity().

Here is the caller graph for this function:

Member Function Documentation

◆ asString() [1/2]

void mrpt::math::TPose3D::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

Definition at line 284 of file TPoseOrPoint.cpp.

References mrpt::format(), pitch, mrpt::RAD2DEG(), roll, and yaw.

Referenced by TEST().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ asString() [2/2]

std::string mrpt::math::TPose3D::asString ( ) const
inline

Definition at line 135 of file TPose3D.h.

◆ asVector()

void mrpt::math::TPose3D::asVector ( std::vector< double > &  v) const
inline

Gets the pose as a vector of doubles.

Definition at line 120 of file TPose3D.h.

References pitch, roll, x, y, yaw, and z.

◆ cols()

constexpr std::size_t mrpt::math::internal::ProvideStaticResize< TPose3D >::cols ( ) const
inlineinherited

Definition at line 57 of file TPoseOrPoint.h.

◆ composePoint() [1/2]

void mrpt::math::TPose3D::composePoint ( const TPoint3D l,
TPoint3D g 
) const

Definition at line 328 of file TPoseOrPoint.cpp.

References getRotationMatrix(), R, x, mrpt::math::TPoint3D_data::x, y, mrpt::math::TPoint3D_data::y, z, and mrpt::math::TPoint3D_data::z.

Referenced by composePoint(), mrpt::math::TPolygon3D::createRegularPolygon(), mrpt::math::project3D(), and unsafeProjectPoint().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ composePoint() [2/2]

TPoint3D mrpt::math::TPose3D::composePoint ( const TPoint3D l) const

Definition at line 339 of file TPoseOrPoint.cpp.

References composePoint().

Here is the call graph for this function:

◆ composePose()

void mrpt::math::TPose3D::composePose ( const TPose3D  other,
TPose3D result 
) const

Definition at line 457 of file TPoseOrPoint.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), fromHomogeneousMatrix(), and getHomogeneousMatrix().

Here is the call graph for this function:

◆ fromHomogeneousMatrix()

void mrpt::math::TPose3D::fromHomogeneousMatrix ( const mrpt::math::CMatrixDouble44 HG)

Definition at line 449 of file TPoseOrPoint.cpp.

References mrpt::math::MatrixVectorBase< Scalar, Derived >::blockCopy(), pitch, roll, SO3_to_yaw_pitch_roll(), and yaw.

Referenced by composePose(), mrpt::math::TPolygon3D::contains(), mrpt::math::TPlane::getAsPose3D(), mrpt::math::TPlane::getAsPose3DForcingOrigin(), mrpt::math::intersect(), mrpt::math::operator-(), and mrpt::math::TPolygonWithPlane::TPolygonWithPlane().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ fromString()

void mrpt::math::TPose3D::fromString ( const std::string s)

Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )

See also
asString
Exceptions
std::exceptionOn invalid format

Definition at line 482 of file TPoseOrPoint.cpp.

References ASSERTMSG_, mrpt::math::CMatrixDynamic< T >::cols(), mrpt::DEG2RAD(), mrpt::math::MatrixVectorBase< Scalar, Derived >::fromMatlabStringFormat(), pitch, roll, mrpt::math::CMatrixDynamic< T >::rows(), THROW_EXCEPTION, and yaw.

Here is the call graph for this function:

◆ getAsQuaternion()

void mrpt::math::TPose3D::getAsQuaternion ( mrpt::math::CQuaternion< double > &  q,
mrpt::math::CMatrixFixed< double, 4, 3 > *  out_dq_dr = nullptr 
) const

Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)

\[ \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) \]

With : $ \phi = roll $, $ \theta = pitch $ and $ \psi = yaw $.

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

Definition at line 290 of file TPoseOrPoint.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::loadFromArray(), pitch, roll, and yaw.

Referenced by mrpt::poses::CPose3D::getAsQuaternion(), mrpt::math::slerp(), and mrpt::math::slerp_ypr().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getHomogeneousMatrix() [1/2]

void mrpt::math::TPose3D::getHomogeneousMatrix ( mrpt::math::CMatrixDouble44 HG) const

Definition at line 465 of file TPoseOrPoint.cpp.

References mrpt::math::MatrixVectorBase< Scalar, Derived >::block(), getRotationMatrix(), R, x, y, and z.

Referenced by composePose(), createPlaneFromPoseAndAxis(), mrpt::math::createPlaneFromPoseAndNormal(), mrpt::math::project3D(), and TEST().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getHomogeneousMatrix() [2/2]

mrpt::math::CMatrixDouble44 mrpt::math::TPose3D::getHomogeneousMatrix ( ) const
inline

Definition at line 174 of file TPose3D.h.

Referenced by composePose(), and getInverseHomogeneousMatrix().

Here is the caller graph for this function:

◆ getInverseHomogeneousMatrix() [1/2]

void mrpt::math::TPose3D::getInverseHomogeneousMatrix ( mrpt::math::CMatrixDouble44 HG) const

Definition at line 476 of file TPoseOrPoint.cpp.

References getHomogeneousMatrix(), and mrpt::math::homogeneousMatrixInverse().

Referenced by mrpt::math::TPolygon3D::contains(), mrpt::math::intersect(), mrpt::math::project3D(), and mrpt::math::TPolygonWithPlane::TPolygonWithPlane().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getInverseHomogeneousMatrix() [2/2]

mrpt::math::CMatrixDouble44 mrpt::math::TPose3D::getInverseHomogeneousMatrix ( ) const
inline

Definition at line 181 of file TPose3D.h.

Referenced by inverseComposePoint().

Here is the caller graph for this function:

◆ getRotationMatrix() [1/2]

void mrpt::math::TPose3D::getRotationMatrix ( mrpt::math::CMatrixDouble33 R) const

Definition at line 364 of file TPoseOrPoint.cpp.

References pitch, R, roll, and yaw.

Referenced by mrpt::math::TTwist3D::rotate().

Here is the caller graph for this function:

◆ getRotationMatrix() [2/2]

mrpt::math::CMatrixDouble33 mrpt::math::TPose3D::getRotationMatrix ( ) const
inline

Definition at line 167 of file TPose3D.h.

References R.

Referenced by composePoint(), and getHomogeneousMatrix().

Here is the caller graph for this function:

◆ Identity()

static constexpr TPose3D mrpt::math::TPose3D::Identity ( )
inlinestatic

Returns the identity transformation, T=eye(4)

Definition at line 40 of file TPose3D.h.

References TPose3D().

Here is the call graph for this function:

◆ inverseComposePoint() [1/2]

void mrpt::math::TPose3D::inverseComposePoint ( const TPoint3D g,
TPoint3D l 
) const

Definition at line 346 of file TPoseOrPoint.cpp.

References getInverseHomogeneousMatrix().

Referenced by inverseComposePoint().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ inverseComposePoint() [2/2]

TPoint3D mrpt::math::TPose3D::inverseComposePoint ( const TPoint3D g) const

Definition at line 357 of file TPoseOrPoint.cpp.

References inverseComposePoint().

Here is the call graph for this function:

◆ norm()

double mrpt::math::TPose3D::norm ( ) const
inline

Pose's spatial coordinates norm.

Definition at line 116 of file TPose3D.h.

References mrpt::square().

Here is the call graph for this function:

◆ operator[]() [1/2]

double& mrpt::math::TPose3D::operator[] ( size_t  i)
inline

Coordinate access using operator[].

Order: x,y,z,yaw,pitch,roll

Definition at line 72 of file TPose3D.h.

References pitch, roll, x, y, yaw, and z.

◆ operator[]() [2/2]

constexpr const double& mrpt::math::TPose3D::operator[] ( size_t  i) const
inline

Coordinate access using operator[].

Order: x,y,z,yaw,pitch,roll

Definition at line 93 of file TPose3D.h.

References pitch, roll, x, y, yaw, and z.

◆ resize()

void mrpt::math::internal::ProvideStaticResize< TPose3D >::resize ( std::size_t  n)
inlineinherited

throws if attempted to resize to incorrect length

Definition at line 61 of file TPoseOrPoint.h.

◆ rows()

constexpr std::size_t mrpt::math::internal::ProvideStaticResize< TPose3D >::rows ( ) const
inlineinherited

Definition at line 56 of file TPoseOrPoint.h.

◆ size()

constexpr std::size_t mrpt::math::internal::ProvideStaticResize< TPose3D >::size ( ) const
inlineinherited

Definition at line 58 of file TPoseOrPoint.h.

◆ SO3_to_yaw_pitch_roll()

void mrpt::math::TPose3D::SO3_to_yaw_pitch_roll ( const mrpt::math::CMatrixDouble33 R,
double &  yaw,
double &  pitch,
double &  roll 
)
static

Definition at line 385 of file TPoseOrPoint.cpp.

References ASSERTDEBMSG_, pitch, R, roll, mrpt::square(), and yaw.

Referenced by fromHomogeneousMatrix().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ pitch

◆ roll

◆ x

◆ y

◆ yaw

◆ z




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8a2d96f36 Mon Jul 15 00:01:58 2019 +0200 at lun jul 15 00:10:13 CEST 2019