MRPT  2.0.4
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 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]

TPose3D::TPose3D ( const TPoint2D p)

Implicit constructor from TPoint2D.

Zeroes all the unprovided information.

See also
TPoint2D

Definition at line 24 of file TPose3D.cpp.

◆ TPose3D() [2/5]

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 28 of file TPose3D.cpp.

◆ TPose3D() [3/5]

TPose3D::TPose3D ( const TPoint3D p)

Implicit constructor from TPoint3D.

Zeroes angular information.

See also
TPoint3D

Definition at line 32 of file TPose3D.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 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
Examples:
comms_nodelets_example/NodeletsTest_impl.cpp.

Definition at line 36 of file TPose3D.cpp.

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

Referenced by onNewMsg(), onNewMsg2(), and 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 66 of file TPoseOrPoint.h.

◆ composePoint() [1/2]

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

Definition at line 80 of file TPose3D.cpp.

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

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

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

◆ composePoint() [2/2]

TPoint3D TPose3D::composePoint ( const TPoint3D l) const

Definition at line 91 of file TPose3D.cpp.

References composePoint().

Here is the call graph for this function:

◆ composePose()

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

Definition at line 209 of file TPose3D.cpp.

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

Referenced by mrpt::vision::recompute_errors_and_Jacobians().

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

◆ fromHomogeneousMatrix()

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

Definition at line 201 of file TPose3D.cpp.

References mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), pitch, roll, SO3_to_yaw_pitch_roll(), x, y, yaw, and z.

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 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 234 of file TPose3D.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, x, y, yaw, and z.

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 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 42 of file TPose3D.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::loadFromArray(), pitch, roll, mrpt::math::CQuaternion< T >::w(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), yaw, and mrpt::math::CQuaternion< T >::z().

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 TPose3D::getHomogeneousMatrix ( mrpt::math::CMatrixDouble44 HG) const

Definition at line 217 of file TPose3D.cpp.

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

Referenced by composePose(), createFromPoseAndAxis(), mrpt::math::createFromPoseAndVector(), createPlaneFromPoseAndAxis(), mrpt::math::createPlaneFromPoseAndNormal(), mrpt::math::operator-(), 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 TPose3D::getInverseHomogeneousMatrix ( mrpt::math::CMatrixDouble44 HG) const

Definition at line 228 of file TPose3D.cpp.

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

Referenced by mrpt::math::TPolygon3D::contains(), mrpt::math::intersect(), mrpt::math::operator-(), 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 TPose3D::getRotationMatrix ( mrpt::math::CMatrixDouble33 R) const

Definition at line 116 of file TPose3D.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 TPose3D::inverseComposePoint ( const TPoint3D g,
TPoint3D l 
) const

Definition at line 98 of file TPose3D.cpp.

References getInverseHomogeneousMatrix(), mrpt::math::TPoint3D_data< T >::x, mrpt::math::TPoint3D_data< T >::y, and mrpt::math::TPoint3D_data< T >::z.

Referenced by inverseComposePoint().

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

◆ inverseComposePoint() [2/2]

TPoint3D TPose3D::inverseComposePoint ( const TPoint3D g) const

Definition at line 109 of file TPose3D.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(), x, y, and z.

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 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 70 of file TPoseOrPoint.h.

◆ rows()

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

Definition at line 65 of file TPoseOrPoint.h.

◆ size()

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

Definition at line 67 of file TPoseOrPoint.h.

◆ SO3_to_yaw_pitch_roll()

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

Definition at line 137 of file TPose3D.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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020