MRPT  1.9.9
List of all members | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes
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 24 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::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >> out_dq_dr=std::nullopt) 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)
 
static TPose3D FromString (const std::string &s)
 

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 27 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 246 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 250 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 254 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 62 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 258 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 136 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 121 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 302 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 313 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 431 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 423 of file TPoseOrPoint.cpp.

References mrpt::math::MatrixBase< 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 456 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.

Referenced by FromString().

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

◆ FromString()

static TPose3D mrpt::math::TPose3D::FromString ( const std::string s)
inlinestatic

Definition at line 200 of file TPose3D.h.

References fromString().

Here is the call graph for this function:

◆ getAsQuaternion()

void mrpt::math::TPose3D::getAsQuaternion ( mrpt::math::CQuaternion< double > &  q,
mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >>  out_dq_dr = std::nullopt 
) 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 264 of file TPoseOrPoint.cpp.

References pitch, roll, and yaw.

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

Here is the caller graph for this function:

◆ getHomogeneousMatrix() [1/2]

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

Definition at line 439 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 176 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 450 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 183 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 338 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 169 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 41 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 320 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 331 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 117 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 73 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 94 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 359 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

double mrpt::math::TPose3D::pitch {.0}

◆ roll

double mrpt::math::TPose3D::roll {.0}

◆ x

double mrpt::math::TPose3D::x {.0}

◆ y

double mrpt::math::TPose3D::y {.0}

◆ yaw

double mrpt::math::TPose3D::yaw {.0}

◆ z

double mrpt::math::TPose3D::z {.0}



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019