A class used to store a 3D pose (a 3D translation + a rotation in 3D).
The 6D transformation in SE(3) stored in this class is kept in two separate containers: a 3-array for the translation, and a 3x3 rotation matrix.
The 6D pose is parameterized as a 6-vector: [x y z yaw pitch roll]. Note however, that the yaw/pitch/roll angles are only computed (on-demand and transparently) when the user requests them. Normally, rotations are handled via the 3x3 rotation matrix only.
For a complete descriptionan of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer to the 2D/3D Geometry tutorial in the wiki.
To change the individual components of the pose, use CPose3D::setFromValues. This class assures that the internal 3x3 rotation matrix is always up-to-date with the "yaw pitch roll" members.
Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.
This class and CPose3DQuat are very similar, and they can be converted to the each other automatically via transformation constructors.
There are Lie algebra methods: exp and ln (see the methods for documentation).
Definition at line 71 of file CPose3D.h.
#include <mrpt/poses/CPose3D.h>
Public Types | |
enum | { is_3D_val = 1 } |
enum | { rotation_dimensions = 3 } |
enum | { is_PDF_val = 0 } |
typedef CPose3D | type_value |
Used to emulate CPosePDF types, for example, in CNetworkOfPoses. | |
Public Member Functions | |
Constructors | |
CPose3D () | |
Default constructor, with all the coordinates set to zero. | |
CPose3D (const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0) | |
Constructor with initilization of the pose; (remember that angles are always given in radians!) | |
CPose3D (const math::CMatrixDouble &m) | |
Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than or equal 3x4, since only those first values are used (the last row of a homogeneous 4x4 matrix are always fixed). | |
CPose3D (const math::CMatrixDouble44 &m) | |
Constructor from a 4x4 homogeneous matrix: | |
template<class MATRIX33 , class VECTOR3 > | |
CPose3D (const MATRIX33 &rot, const VECTOR3 &xyz) | |
Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a TPoint3D. | |
CPose3D (const CMatrixDouble33 &rot, const CArrayDouble< 3 > &xyz) | |
CPose3D (const CPose2D &) | |
Constructor from a CPose2D object. | |
CPose3D (const CPoint3D &) | |
Constructor from a CPoint3D object. | |
CPose3D (const mrpt::math::TPose3D &) | |
Constructor from lightweight object. | |
CPose3D (const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z) | |
Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. | |
CPose3D (const CPose3DQuat &) | |
Constructor from a CPose3DQuat. | |
CPose3D (TConstructorFlags_Poses constructor_dummy_param) | |
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument. | |
CPose3D (const CArrayDouble< 12 > &vec12) | |
Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose. | |
Access 3x3 rotation and 4x4 homogeneous matrices | |
void | getHomogeneousMatrix (CMatrixDouble44 &out_HM) const |
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). | |
CMatrixDouble44 | getHomogeneousMatrixVal () const |
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). | |
void | getRotationMatrix (mrpt::math::CMatrixDouble33 &ROT) const |
Get the 3x3 rotation matrix. | |
const mrpt::math::CMatrixDouble33 & | getRotationMatrix () const |
Pose-pose and pose-point compositions and operators | |
CPose3D | operator+ (const CPose3D &b) const |
The operator ![]() | |
CPoint3D | operator+ (const CPoint3D &b) const |
The operator ![]() | |
CPoint3D | operator+ (const CPoint2D &b) const |
The operator ![]() | |
void | sphericalCoordinates (const TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const |
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. | |
void | composePoint (double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const |
An alternative, slightly more efficient way of doing ![]() | |
void | composePoint (const TPoint3D local_point, TPoint3D &global_point) const |
An alternative, slightly more efficient way of doing ![]() | |
void | composePoint (double lx, double ly, double lz, float &gx, float &gy, float &gz) const |
An alternative, slightly more efficient way of doing ![]() | |
void | inverseComposePoint (const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL) const |
Computes the 3D point L such as ![]() | |
void | composeFrom (const CPose3D &A, const CPose3D &B) |
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object. | |
CPose3D & | operator+= (const CPose3D &b) |
Make ![]() | |
void | inverseComposeFrom (const CPose3D &A, const CPose3D &B) |
Makes ![]() | |
CPose3D | operator- (const CPose3D &b) const |
Compute ![]() | |
void | inverse () |
Convert this pose into its inverse, saving the result in itself. | |
void | changeCoordinatesReference (const CPose3D &p) |
makes: this = p (+) this | |
Access and modify contents | |
void | addComponents (const CPose3D &p) |
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators. | |
void | normalizeAngles () |
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents) | |
void | operator*= (const double s) |
Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval). | |
void | setFromValues (const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0) |
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix. | |
template<typename VECTORLIKE > | |
void | setFromXYZQ (const VECTORLIKE &v, const size_t index_offset=0) |
Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-element vector. | |
void | setYawPitchRoll (const double yaw_, const double pitch_, const double roll_) |
Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix. | |
template<class ARRAYORVECTOR > | |
void | setFrom12Vector (const ARRAYORVECTOR &vec12) |
Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose. | |
template<class ARRAYORVECTOR > | |
void | getAs12Vector (ARRAYORVECTOR &vec12) const |
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose. | |
void | getYawPitchRoll (double &yaw, double &pitch, double &roll) const |
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix. | |
double | yaw () const |
Get the YAW angle (in radians) | |
double | pitch () const |
Get the PITCH angle (in radians) | |
double | roll () const |
Get the ROLL angle (in radians) | |
void | getAsVector (vector_double &v) const |
Returns a 1x6 vector with [x y z yaw pitch roll]. | |
void | getAsQuaternion (mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const |
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
| |
const double & | operator[] (unsigned int i) const |
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.) | |
std::string | asString () const |
void | fromString (const std::string &s) |
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg. | |
bool | isHorizontal (const double tolerance=0) const |
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards), with a given tolerance (if set to 0 exact horizontality is tested). | |
double | distanceEuclidean6D (const CPose3D &o) const |
The euclidean distance between two poses taken as two 6-length vectors (angles in radians). | |
Static Public Member Functions | |
static bool | is_3D () |
static bool | is_PDF () |
Public Attributes | |
CArrayDouble< 3 > | m_coords |
The translation vector [x,y,z]. | |
CMatrixDouble33 | m_ROT |
The 3x3 rotation matrix. | |
Protected Member Functions | |
void | rebuildRotationMatrix () |
Rebuild the homog matrix from the angles. | |
void | updateYawPitchRoll () const |
Updates Yaw/pitch/roll members from the m_ROT. | |
Protected Attributes | |
bool | m_ypr_uptodate |
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update. | |
double | m_yaw |
double | m_pitch |
double | m_roll |
These variables are updated every time that the object rotation matrix is modified (construction, loading from values, pose composition, etc ) | |
STL-like methods and typedefs | |
enum | { static_size = 6 } |
typedef double | value_type |
The type of the elements. | |
typedef double & | reference |
typedef const double & | const_reference |
typedef std::size_t | size_type |
typedef std::ptrdiff_t | difference_type |
static size_type | size () |
static bool | empty () |
static size_type | max_size () |
static void | resize (const size_t n) |
RTTI stuff | |
typedef CPose3DPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CPose3D |
static mrpt::utils::TRuntimeClassId | classCPose3D |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const |
Returns information about the class of an object in runtime. | |
virtual mrpt::utils::CObject * | duplicate () const |
Returns a copy of the object, indepently of its class. | |
static mrpt::utils::CObject * | CreateObject () |
static CPose3DPtr | Create () |
Lie Algebra methods | |
void | ln (mrpt::math::CArrayDouble< 6 > &out_ln) const |
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra. | |
mrpt::math::CArrayDouble< 6 > | ln () const |
void | ln_jacob (mrpt::math::CMatrixFixedNumeric< double, 6, 12 > &J) const |
Jacobian of the logarithm of the 3x4 matrix defined by this pose. | |
CArrayDouble< 3 > | ln_rotation () const |
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra. | |
static CPose3D | exp (const mrpt::math::CArrayNumeric< double, 6 > &vect) |
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method). | |
static CMatrixDouble33 | exp_rotation (const mrpt::math::CArrayNumeric< double, 3 > &vect) |
Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix). | |
static void | ln_rot_jacob (const CMatrixDouble33 &R, CMatrixFixedNumeric< double, 3, 9 > &M) |
Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rotation matrix R. |
typedef const double& mrpt::poses::CPose3D::const_reference |
typedef std::ptrdiff_t mrpt::poses::CPose3D::difference_type |
typedef double& mrpt::poses::CPose3D::reference |
typedef std::size_t mrpt::poses::CPose3D::size_type |
Used to emulate CPosePDF types, for example, in CNetworkOfPoses.
typedef double mrpt::poses::CPose3D::value_type |
mrpt::poses::CPose3D::CPose3D | ( | ) |
Default constructor, with all the coordinates set to zero.
mrpt::poses::CPose3D::CPose3D | ( | const double | x, |
const double | y, | ||
const double | z, | ||
const double | yaw = 0 , |
||
const double | pitch = 0 , |
||
const double | roll = 0 |
||
) |
Constructor with initilization of the pose; (remember that angles are always given in radians!)
mrpt::poses::CPose3D::CPose3D | ( | const math::CMatrixDouble & | m ) | [explicit] |
Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than or equal 3x4, since only those first values are used (the last row of a homogeneous 4x4 matrix are always fixed).
mrpt::poses::CPose3D::CPose3D | ( | const math::CMatrixDouble44 & | m ) | [explicit] |
Constructor from a 4x4 homogeneous matrix:
mrpt::poses::CPose3D::CPose3D | ( | const MATRIX33 & | rot, |
const VECTOR3 & | xyz | ||
) | [inline] |
Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a TPoint3D.
Definition at line 108 of file CPose3D.h.
References ASSERT_EQUAL_, and mrpt::math::size().
mrpt::poses::CPose3D::CPose3D | ( | const CMatrixDouble33 & | rot, |
const CArrayDouble< 3 > & | xyz | ||
) | [inline] |
mrpt::poses::CPose3D::CPose3D | ( | const mrpt::math::TPose3D & | ) |
Constructor from lightweight object.
mrpt::poses::CPose3D::CPose3D | ( | const mrpt::math::CQuaternionDouble & | q, |
const double | x, | ||
const double | y, | ||
const double | z | ||
) |
Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement.
mrpt::poses::CPose3D::CPose3D | ( | const CPose3DQuat & | ) |
Constructor from a CPose3DQuat.
mrpt::poses::CPose3D::CPose3D | ( | TConstructorFlags_Poses | constructor_dummy_param ) | [inline] |
mrpt::poses::CPose3D::CPose3D | ( | const CArrayDouble< 12 > & | vec12 ) | [inline, explicit] |
Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose.
static const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3D::_GetBaseClass | ( | ) | [static, protected] |
Reimplemented from mrpt::utils::CSerializable.
void mrpt::poses::CPose3D::addComponents | ( | const CPose3D & | p ) |
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators.
void mrpt::poses::CPose3D::asString | ( | std::string & | s ) | const [inline] |
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
Definition at line 402 of file CPose3D.h.
References mrpt::format(), and mrpt::utils::RAD2DEG().
std::string mrpt::poses::CPose3D::asString | ( | ) | const [inline] |
void mrpt::poses::CPose3D::changeCoordinatesReference | ( | const CPose3D & | p ) | [inline] |
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
Referenced by operator+(), mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_aux_perform_one_rejection_sampling_step(), and mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_particlesEvaluator_AuxPFStandard().
void mrpt::poses::CPose3D::composePoint | ( | const TPoint3D | local_point, |
TPoint3D & | global_point | ||
) | const [inline] |
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose.
Definition at line 215 of file CPose3D.h.
References mrpt::math::TPoint3D::x, mrpt::math::TPoint3D::y, and mrpt::math::TPoint3D::z.
void mrpt::poses::CPose3D::composePoint | ( | double | lx, |
double | ly, | ||
double | lz, | ||
float & | gx, | ||
float & | gy, | ||
float & | gz | ||
) | const [inline] |
void mrpt::poses::CPose3D::composePoint | ( | double | lx, |
double | ly, | ||
double | lz, | ||
double & | gx, | ||
double & | gy, | ||
double & | gz, | ||
mrpt::math::CMatrixFixedNumeric< double, 3, 3 > * | out_jacobian_df_dpoint = NULL , |
||
mrpt::math::CMatrixFixedNumeric< double, 3, 6 > * | out_jacobian_df_dpose = NULL , |
||
mrpt::math::CMatrixFixedNumeric< double, 3, 6 > * | out_jacobian_df_dse3 = NULL , |
||
bool | use_small_rot_approx = false |
||
) | const |
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose.
If pointers are provided, the corresponding Jacobians are returned. "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3). See this report for mathematical details.
If | set to true, the Jacobian "out_jacobian_df_dpose" uses a fastest linearized appoximation (valid only for small rotations!). |
Referenced by mrpt::math::project3D(), and mrpt::vision::pinhole::projectPoint_no_distortion().
static CPose3DPtr mrpt::poses::CPose3D::Create | ( | ) | [static] |
static mrpt::utils::CObject* mrpt::poses::CPose3D::CreateObject | ( | ) | [static] |
double mrpt::poses::CPose3D::distanceEuclidean6D | ( | const CPose3D & | o ) | const |
The euclidean distance between two poses taken as two 6-length vectors (angles in radians).
virtual mrpt::utils::CObject* mrpt::poses::CPose3D::duplicate | ( | ) | const [virtual] |
Returns a copy of the object, indepently of its class.
Implements mrpt::utils::CObject.
static bool mrpt::poses::CPose3D::empty | ( | ) | [inline, static] |
static CPose3D mrpt::poses::CPose3D::exp | ( | const mrpt::math::CArrayNumeric< double, 6 > & | vect ) | [static] |
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
static CMatrixDouble33 mrpt::poses::CPose3D::exp_rotation | ( | const mrpt::math::CArrayNumeric< double, 3 > & | vect ) | [static] |
Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix).
void mrpt::poses::CPose3D::fromString | ( | const std::string & | s ) | [inline] |
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg.
)
std::exception | On invalid format |
Definition at line 409 of file CPose3D.h.
References ASSERTMSG_, mrpt::utils::DEG2RAD(), mrpt::math::size(), and THROW_EXCEPTION.
void mrpt::poses::CPose3D::getAs12Vector | ( | ARRAYORVECTOR & | vec12 ) | const [inline] |
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose.
void mrpt::poses::CPose3D::getAsQuaternion | ( | mrpt::math::CQuaternionDouble & | q, |
mrpt::math::CMatrixFixedNumeric< double, 4, 3 > * | out_dq_dr = NULL |
||
) | 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). |
Referenced by mrpt::math::jacobians::jacob_quat_from_yawpitchroll().
void mrpt::poses::CPose3D::getAsVector | ( | vector_double & | v ) | const |
Returns a 1x6 vector with [x y z yaw pitch roll].
void mrpt::poses::CPose3D::getHomogeneousMatrix | ( | CMatrixDouble44 & | out_HM ) | const [inline] |
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
CMatrixDouble44 mrpt::poses::CPose3D::getHomogeneousMatrixVal | ( | ) | const [inline] |
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
Reimplemented from mrpt::poses::CPoseOrPoint< CPose3D >.
void mrpt::poses::CPose3D::getRotationMatrix | ( | mrpt::math::CMatrixDouble33 & | ROT ) | const [inline] |
Get the 3x3 rotation matrix.
const mrpt::math::CMatrixDouble33& mrpt::poses::CPose3D::getRotationMatrix | ( | ) | const [inline] |
virtual const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3D::GetRuntimeClass | ( | ) | const [virtual] |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::utils::CSerializable.
void mrpt::poses::CPose3D::getYawPitchRoll | ( | double & | yaw, |
double & | pitch, | ||
double & | roll | ||
) | const |
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
void mrpt::poses::CPose3D::inverse | ( | ) |
Convert this pose into its inverse, saving the result in itself.
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
Referenced by operator-().
void mrpt::poses::CPose3D::inverseComposePoint | ( | const double | gx, |
const double | gy, | ||
const double | gz, | ||
double & | lx, | ||
double & | ly, | ||
double & | lz, | ||
mrpt::math::CMatrixFixedNumeric< double, 3, 3 > * | out_jacobian_df_dpoint = NULL , |
||
mrpt::math::CMatrixFixedNumeric< double, 3, 6 > * | out_jacobian_df_dpose = NULL , |
||
mrpt::math::CMatrixFixedNumeric< double, 3, 6 > * | out_jacobian_df_dse3 = NULL |
||
) | const |
Computes the 3D point L such as .
If pointers are provided, the corresponding Jacobians are returned. "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3). See this report for mathematical details.
Referenced by mrpt::vision::pinhole::projectPoint_no_distortion().
static bool mrpt::poses::CPose3D::is_3D | ( | ) | [inline, static] |
static bool mrpt::poses::CPose3D::is_PDF | ( | ) | [inline, static] |
bool mrpt::poses::CPose3D::isHorizontal | ( | const double | tolerance = 0 ) |
const |
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards), with a given tolerance (if set to 0 exact horizontality is tested).
void mrpt::poses::CPose3D::ln | ( | mrpt::math::CArrayDouble< 6 > & | out_ln ) | const |
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra.
Referenced by mrpt::poses::SE_traits< 3 >::ln().
mrpt::math::CArrayDouble<6> mrpt::poses::CPose3D::ln | ( | ) | const [inline] |
void mrpt::poses::CPose3D::ln_jacob | ( | mrpt::math::CMatrixFixedNumeric< double, 6, 12 > & | J ) | const |
Jacobian of the logarithm of the 3x4 matrix defined by this pose.
static void mrpt::poses::CPose3D::ln_rot_jacob | ( | const CMatrixDouble33 & | R, |
CMatrixFixedNumeric< double, 3, 9 > & | M | ||
) | [static] |
CArrayDouble<3> mrpt::poses::CPose3D::ln_rotation | ( | ) | const |
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra.
static size_type mrpt::poses::CPose3D::max_size | ( | ) | [inline, static] |
Definition at line 484 of file CPose3D.h.
References static_size.
void mrpt::poses::CPose3D::normalizeAngles | ( | ) |
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)
void mrpt::poses::CPose3D::operator*= | ( | const double | s ) |
Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).
The operator is the pose compounding operator.
Definition at line 180 of file CPose3D.h.
References composeFrom(), and mrpt::poses::UNINITIALIZED_POSE.
The operator is the pose compounding operator.
The operator is the pose compounding operator.
Compute .
Reimplemented from mrpt::poses::CPose< CPose3D >.
Definition at line 256 of file CPose3D.h.
References inverseComposeFrom(), and mrpt::poses::UNINITIALIZED_POSE.
const double& mrpt::poses::CPose3D::operator[] | ( | unsigned int | i ) | const [inline] |
double mrpt::poses::CPose3D::pitch | ( | ) | const [inline] |
void mrpt::poses::CPose3D::rebuildRotationMatrix | ( | ) | [protected] |
Rebuild the homog matrix from the angles.
static void mrpt::poses::CPose3D::resize | ( | const size_t | n ) | [inline, static] |
Definition at line 485 of file CPose3D.h.
References mrpt::format(), and static_size.
double mrpt::poses::CPose3D::roll | ( | ) | const [inline] |
void mrpt::poses::CPose3D::setFrom12Vector | ( | const ARRAYORVECTOR & | vec12 ) | [inline] |
Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose.
void mrpt::poses::CPose3D::setFromValues | ( | const double | x0, |
const double | y0, | ||
const double | z0, | ||
const double | yaw = 0 , |
||
const double | pitch = 0 , |
||
const double | roll = 0 |
||
) |
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.
void mrpt::poses::CPose3D::setFromXYZQ | ( | const VECTORLIKE & | v, |
const size_t | index_offset = 0 |
||
) | [inline] |
Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-element vector.
Definition at line 303 of file CPose3D.h.
References ASSERT_ABOVEEQ_.
void mrpt::poses::CPose3D::setYawPitchRoll | ( | const double | yaw_, |
const double | pitch_, | ||
const double | roll_ | ||
) | [inline] |
Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix.
Definition at line 320 of file CPose3D.h.
References internal::y.
static size_type mrpt::poses::CPose3D::size | ( | ) | [inline, static] |
Definition at line 482 of file CPose3D.h.
References static_size.
void mrpt::poses::CPose3D::sphericalCoordinates | ( | const TPoint3D & | point, |
double & | out_range, | ||
double & | out_yaw, | ||
double & | out_pitch | ||
) | const |
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
For the coordinate system see the top of this page.
void mrpt::poses::CPose3D::updateYawPitchRoll | ( | ) | const [inline, protected] |
double mrpt::poses::CPose3D::yaw | ( | ) | const [inline] |
mrpt::utils::CLASSINIT mrpt::poses::CPose3D::_init_CPose3D [static, protected] |
const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3D::classinfo [static] |
double mrpt::poses::CPose3D::m_pitch [mutable, protected] |
double mrpt::poses::CPose3D::m_roll [mutable, protected] |
double mrpt::poses::CPose3D::m_yaw [mutable, protected] |
bool mrpt::poses::CPose3D::m_ypr_uptodate [mutable, protected] |
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |