MRPT
1.9.9
|
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Allows coordinate access using [] operator.
#include <mrpt/math/TPose3D.h>
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)
With : , and . 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... | |
TPose3D::TPose3D | ( | const TPoint2D & | p | ) |
Implicit constructor from TPoint2D.
Zeroes all the unprovided information.
Definition at line 24 of file TPose3D.cpp.
TPose3D::TPose3D | ( | const TPose2D & | p | ) |
Implicit constructor from TPose2D.
Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
Definition at line 28 of file TPose3D.cpp.
TPose3D::TPose3D | ( | const TPoint3D & | p | ) |
Implicit constructor from TPoint3D.
Zeroes angular information.
Definition at line 32 of file TPose3D.cpp.
|
inline |
|
default |
Default fast constructor.
Initializes to zeros.
Referenced by Identity().
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.)
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().
|
inline |
|
inline |
|
inlineinherited |
Definition at line 66 of file TPoseOrPoint.h.
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().
Definition at line 91 of file TPose3D.cpp.
References composePoint().
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().
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().
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]" )
std::exception | On 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().
|
inlinestatic |
Definition at line 200 of file TPose3D.h.
References fromString().
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)
With : , and .
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 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().
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().
|
inline |
Definition at line 176 of file TPose3D.h.
Referenced by composePose(), and getInverseHomogeneousMatrix().
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().
|
inline |
Definition at line 183 of file TPose3D.h.
Referenced by inverseComposePoint().
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().
|
inline |
Definition at line 169 of file TPose3D.h.
References R.
Referenced by composePoint(), and getHomogeneousMatrix().
|
inlinestatic |
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().
Definition at line 109 of file TPose3D.cpp.
References inverseComposePoint().
|
inline |
|
inline |
|
inline |
|
inlineinherited |
throws if attempted to resize to incorrect length
Definition at line 70 of file TPoseOrPoint.h.
|
inlineinherited |
Definition at line 65 of file TPoseOrPoint.h.
|
inlineinherited |
Definition at line 67 of file TPoseOrPoint.h.
|
static |
Definition at line 137 of file TPose3D.cpp.
References ASSERTDEBMSG_, pitch, R, roll, mrpt::square(), and yaw.
Referenced by fromHomogeneousMatrix().
double mrpt::math::TPose3D::pitch {.0} |
Pitch coordinate (rotation angle over Y axis).
Definition at line 36 of file TPose3D.h.
Referenced by asString(), asVector(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), getAsQuaternion(), mrpt::poses::CPose3DGridTemplate< double >::getByPos(), 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::topography::path_from_rtk_gps(), mrpt::poses::CPose3DGridTemplate< double >::pitch2idx(), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(), mrpt::math::slerp(), mrpt::math::slerp_ypr(), SO3_to_yaw_pitch_roll(), and TEST().
double mrpt::math::TPose3D::roll {.0} |
Roll coordinate (rotation angle over X coordinate).
Definition at line 38 of file TPose3D.h.
Referenced by asString(), asVector(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), getAsQuaternion(), mrpt::poses::CPose3DGridTemplate< double >::getByPos(), 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::topography::path_from_rtk_gps(), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::poses::CPose3DGridTemplate< double >::roll2idx(), mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(), mrpt::math::slerp(), mrpt::math::slerp_ypr(), SO3_to_yaw_pitch_roll(), and TEST().
double mrpt::math::TPose3D::x {.0} |
X,Y,Z, coords.
Definition at line 32 of file TPose3D.h.
Referenced by asString(), asVector(), composePoint(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), mrpt::poses::CPose3DGridTemplate< double >::getByPos(), getHomogeneousMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2x(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), norm(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), pf2gauss_t< mrpt::slam::CMonteCarloLocalization2D >::resetOnFreeSpace(), mrpt::poses::CPose3DPDFParticles::resetUniform(), pf2gauss_t< mrpt::slam::CMonteCarloLocalization2D >::resetUniform(), mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(), mrpt::math::slerp(), mrpt::math::slerp_ypr(), TEST(), mrpt::math::TPoint2D_< double >::TPoint2D_(), mrpt::math::TPoint3D_< float >::TPoint3D_(), and mrpt::poses::CPose3DGridTemplate< double >::x2idx().
double mrpt::math::TPose3D::y {.0} |
Definition at line 32 of file TPose3D.h.
Referenced by asString(), asVector(), composePoint(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), mrpt::poses::CPose3DGridTemplate< double >::getByPos(), getHomogeneousMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2y(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), norm(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), pf2gauss_t< mrpt::slam::CMonteCarloLocalization2D >::resetOnFreeSpace(), mrpt::poses::CPose3DPDFParticles::resetUniform(), pf2gauss_t< mrpt::slam::CMonteCarloLocalization2D >::resetUniform(), mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(), mrpt::math::slerp(), mrpt::math::slerp_ypr(), TEST(), mrpt::math::TPoint2D_< double >::TPoint2D_(), mrpt::math::TPoint3D_< float >::TPoint3D_(), and mrpt::poses::CPose3DGridTemplate< double >::y2idx().
double mrpt::math::TPose3D::yaw {.0} |
Yaw coordinate (rotation angle over Z axis).
Definition at line 34 of file TPose3D.h.
Referenced by asString(), asVector(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), getAsQuaternion(), mrpt::poses::CPose3DGridTemplate< double >::getByPos(), 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[](), pf2gauss_t< mrpt::slam::CMonteCarloLocalization2D >::resetOnFreeSpace(), mrpt::poses::CPose3DPDFParticles::resetUniform(), pf2gauss_t< mrpt::slam::CMonteCarloLocalization2D >::resetUniform(), mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(), mrpt::math::slerp(), mrpt::math::slerp_ypr(), SO3_to_yaw_pitch_roll(), TEST(), and mrpt::poses::CPose3DGridTemplate< double >::yaw2idx().
double mrpt::math::TPose3D::z {.0} |
Definition at line 32 of file TPose3D.h.
Referenced by asString(), asVector(), composePoint(), mrpt::poses::CPose3D::CPose3D(), fromHomogeneousMatrix(), fromString(), mrpt::poses::CPose3DGridTemplate< double >::getByPos(), getHomogeneousMatrix(), mrpt::poses::CPose3DGridTemplate< double >::idx2z(), mrpt::slam::KLF_loadBinFromParticle(), mrpt::graphs::detail::graph_ops< graph_t >::load_graph_of_poses_from_text_stream(), norm(), mrpt::math::operator!=(), mrpt::math::operator==(), operator[](), mrpt::poses::CPose3DPDFParticles::resetUniform(), mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(), mrpt::math::slerp(), mrpt::math::slerp_ypr(), TEST(), mrpt::math::TPoint3D_< float >::TPoint3D_(), and mrpt::poses::CPose3DGridTemplate< double >::z2idx().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020 |