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::exception On 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_dr If 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

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

Pitch coordinate (rotation angle over Y axis).

Definition at line 35 of file TPose3D.h.

Referenced by asString(), asVector(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), getAsQuaternion(), getRotationMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2pitch(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DGridTemplate< double >::pitch2idx(), mrpt::poses::CPose3DPDFParticles::resetUniform(), and SO3_to_yaw_pitch_roll().

◆ roll

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

Roll coordinate (rotation angle over X coordinate).

Definition at line 37 of file TPose3D.h.

Referenced by asString(), asVector(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), getAsQuaternion(), getRotationMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2roll(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::poses::CPose3DGridTemplate< double >::roll2idx(), and SO3_to_yaw_pitch_roll().

◆ x

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

X,Y,Z, coords.

Definition at line 31 of file TPose3D.h.

Referenced by asVector(), composePoint(), mrpt::poses::CPose3D::CPose3D(), getHomogeneousMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2x(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::math::slerp(), and mrpt::poses::CPose3DGridTemplate< double >::x2idx().

◆ y

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

Definition at line 31 of file TPose3D.h.

Referenced by asVector(), composePoint(), mrpt::poses::CPose3D::CPose3D(), getHomogeneousMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2y(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::math::slerp(), and mrpt::poses::CPose3DGridTemplate< double >::y2idx().

◆ yaw

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

Yaw coordinate (rotation angle over Z axis).

Definition at line 33 of file TPose3D.h.

Referenced by asString(), asVector(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), getAsQuaternion(), getRotationMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2yaw(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DPDFParticles::resetUniform(), SO3_to_yaw_pitch_roll(), and mrpt::poses::CPose3DGridTemplate< double >::yaw2idx().

◆ z

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

Definition at line 31 of file TPose3D.h.

Referenced by asVector(), composePoint(), mrpt::poses::CPose3D::CPose3D(), getHomogeneousMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2z(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::math::slerp(), and mrpt::poses::CPose3DGridTemplate< double >::z2idx().

Detailed Description

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ TPose3D() [1/5]

◆ TPose3D() [2/5]

◆ TPose3D() [3/5]

◆ TPose3D() [4/5]

◆ TPose3D() [5/5]

Member Function Documentation

◆ asString() [1/2]

◆ asString() [2/2]

◆ asVector()

◆ cols()

◆ composePoint() [1/2]

◆ composePoint() [2/2]

◆ composePose()

◆ fromHomogeneousMatrix()

◆ fromString()

◆ getAsQuaternion()

◆ getHomogeneousMatrix() [1/2]

◆ getHomogeneousMatrix() [2/2]

◆ getInverseHomogeneousMatrix() [1/2]

◆ getInverseHomogeneousMatrix() [2/2]

◆ getRotationMatrix() [1/2]

◆ getRotationMatrix() [2/2]

◆ Identity()

◆ inverseComposePoint() [1/2]

◆ inverseComposePoint() [2/2]

◆ norm()

◆ operator[]() [1/2]

◆ operator[]() [2/2]

◆ resize()

◆ rows()

◆ size()

◆ SO3_to_yaw_pitch_roll()

Member Data Documentation

◆ pitch

◆ roll

◆ x

◆ y

◆ yaw

◆ z