MRPT logo

mrpt::poses::CPose3D Class Reference

A class used to store a 3D pose. More...

#include <mrpt/poses/CPose3D.h>

Inheritance diagram for mrpt::poses::CPose3D:

mrpt::poses::CPose mrpt::poses::CPoseOrPoint mrpt::utils::CSerializable

List of all members.

Public Member Functions

 CPose3D (const double &x=0, const double &y=0, const double &z=0, 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 CPose3D &o)
 Copy constructor.
CPose3Doperator= (const CPose3D &o)
 Copy operator.
 CPose3D (const math::CMatrixDouble &m)
 Constructor from a 4x4 homogeneous matrix:.
 CPose3D (const math::CMatrixDouble44 &m)
 Constructor from a 4x4 homogeneous matrix:.
 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.
void getHomogeneousMatrix (CMatrixDouble44 &out_HM) const
 Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
CPose3D operator+ (const CPose3D &b) const
 The operator $ a \oplus b $ is the pose compounding operator.
CPoint3D operator+ (const CPoint3D &b) const
 The operator $ a \oplus b $ is the pose compounding operator.
CPoint3D operator+ (const CPoint2D &b) const
 The operator $ a \oplus b $ is the pose compounding operator.
void addComponents (const CPose3D &p)
 Scalar sum of 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 sphericalCoordinates (const CPoint3D &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) const
 An alternative, slightly more efficient way of doing $ G = P \oplus L $ with G and L being 3D points and P this 6D pose.
void composePoint (float lx, float ly, float lz, float &gx, float &gy, float &gz) const
 An alternative, slightly more efficient way of doing $ G = P \oplus L $ with G and L being 3D points and P this 6D pose.
void composePoint (const TPoint3D local_point, TPoint3D &global_point) const
 An alternative, slightly more efficient way of doing $ G = P \oplus L $ with G and L being 3D points and P this 6D pose.
double x () const
 Get the X coordinate.
double y () const
 Get the Y coordinate.
double z () const
 Get the Z coordinate.
void x (const double x_)
 Set the X coordinate.
void y (const double y_)
 Set the Y coordinate.
void z (const double z_)
 Set the Z coordinate.
void x_incr (const double Ax)
 Increment the X coordinate.
void y_incr (const double Ay)
 Increment the Y coordinate.
void z_incr (const double Az)
 Increment the Z coordinate.
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 homogeneous coordinates matrix.
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 homogeneous coordinates matrix.
void getYawPitchRoll (double &yaw, double &pitch, double &roll)
 Returns the three angles (yaw, pitch, roll), in radians, from the homogeneous 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).
double distanceEuclidean6D (const CPose3D &o) const
 The euclidean distance between two poses taken as two 6-length vectors (angles 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) 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 $.

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.
CPose3Doperator+= (const CPose3D &b)
 Make $ this = this \oplus b $.

Protected Member Functions

void rebuildHomogeneousMatrix ()
 Rebuild the homog matrix from x,y,z and the angles.

Protected Attributes

double m_yaw
double m_pitch
double m_roll
 These variables are updated every time that the object homogeneous matrix is modified (construction, loading from values, pose composition, etc ).
CMatrixDouble44 m_HM
 The homogeneous matrix.

Friends

class CPose
class CPose2D
class CPoint
std::ostream MRPTDLLIMPEXP & operator<< (std::ostream &o, const CPose3D &p)


Detailed Description

A class used to store a 3D pose.

A class used to store a 3D (6D) pose, including the 3D coordinate point and orientation angles. It is used in many situations, from defining a robot pose, maps relative poses, sensors,... See introduction in documentation for the CPoseOrPoint class.
For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint

For a complete description 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 4x4 homogeneous coordinate matrix is always up-to-date with the "x y z yaw pitch roll" members.

Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.

poses::CPose3D

Homogeneous transfomation matrix

Spatial representation

cycp

cyspsr-sycr

cyspcr+sysr

x

sycp

syspsr+cycr

syspcr-cysr

y

-sp

cpsr

cpcr

z

0

0

0

1

where:

cy = cos Yaw ;  sy = sin Yaw

cp = cos Pitch ; sp = sin Pitch

cr = cos Roll ; sr = sin Roll

CPose3D.gif

See also:
CPoseOrPoint,CPoint3D

Definition at line 164 of file CPose3D.h.


Constructor & Destructor Documentation

mrpt::poses::CPose3D::CPose3D ( const double &  x = 0,
const double &  y = 0,
const double &  z = 0,
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 CPose3D o  ) 

Copy constructor.

mrpt::poses::CPose3D::CPose3D ( const math::CMatrixDouble m  )  [explicit]

Constructor from a 4x4 homogeneous matrix:.

mrpt::poses::CPose3D::CPose3D ( const math::CMatrixDouble44 m  )  [explicit]

Constructor from a 4x4 homogeneous matrix:.

mrpt::poses::CPose3D::CPose3D ( const CPose2D  ) 

Constructor from a CPose2D object.

mrpt::poses::CPose3D::CPose3D ( const CPoint3D  ) 

Constructor from a CPoint3D object.

mrpt::poses::CPose3D::CPose3D ( const mrpt::math::TPose3D  ) 

Constructor from lightweight object.


Member Function Documentation

void mrpt::poses::CPose3D::addComponents ( const CPose3D p  ) 

Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operators.

See also:
normalizeAngles

void mrpt::poses::CPose3D::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.

Note:
A or B can be "this" without problems.

void mrpt::poses::CPose3D::composePoint ( const TPoint3D  local_point,
TPoint3D global_point 
) const

An alternative, slightly more efficient way of doing $ G = P \oplus L $ with G and L being 3D points and P this 6D pose.

Note:
local_point is passed by value to allow global and local point to be the same variable

void mrpt::poses::CPose3D::composePoint ( float  lx,
float  ly,
float  lz,
float &  gx,
float &  gy,
float &  gz 
) const

An alternative, slightly more efficient way of doing $ G = P \oplus L $ with G and L being 3D points and P this 6D pose.

void mrpt::poses::CPose3D::composePoint ( double  lx,
double  ly,
double  lz,
double &  gx,
double &  gy,
double &  gz 
) const

An alternative, slightly more efficient way of doing $ G = P \oplus L $ with G and L being 3D points and P this 6D pose.

Referenced by mrpt::math::project3D().

double mrpt::poses::CPose3D::distanceEuclidean6D ( const CPose3D o  )  const

The euclidean distance between two poses taken as two 6-length vectors (angles in radians).

void mrpt::poses::CPose3D::getAsQuaternion ( mrpt::math::CQuaternionDouble q  )  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 $.

void mrpt::poses::CPose3D::getAsVector ( vector_double v  )  const [virtual]

Returns a 1x6 vector with [x y z yaw pitch roll].

Implements mrpt::poses::CPoseOrPoint.

void mrpt::poses::CPose3D::getHomogeneousMatrix ( CMatrixDouble44 out_HM  )  const [inline, virtual]

Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).

See also:
getInverseHomogeneousMatrix

Implements mrpt::poses::CPoseOrPoint.

Definition at line 215 of file CPose3D.h.

void mrpt::poses::CPose3D::getYawPitchRoll ( double &  yaw,
double &  pitch,
double &  roll 
)

Returns the three angles (yaw, pitch, roll), in radians, from the homogeneous matrix.

See also:
setFromValues, yaw, pitch, roll

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).

See also:
addComponents

void mrpt::poses::CPose3D::operator*= ( const double &  s  )  [virtual]

Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).

Implements mrpt::poses::CPoseOrPoint.

CPoint3D mrpt::poses::CPose3D::operator+ ( const CPoint2D b  )  const

The operator $ a \oplus b $ is the pose compounding operator.

CPoint3D mrpt::poses::CPose3D::operator+ ( const CPoint3D b  )  const

The operator $ a \oplus b $ is the pose compounding operator.

CPose3D mrpt::poses::CPose3D::operator+ ( const CPose3D b  )  const

The operator $ a \oplus b $ is the pose compounding operator.

CPose3D& mrpt::poses::CPose3D::operator+= ( const CPose3D b  )  [inline]

Make $ this = this \oplus b $.

Definition at line 330 of file CPose3D.h.

CPose3D& mrpt::poses::CPose3D::operator= ( const CPose3D o  ) 

Copy operator.

double mrpt::poses::CPose3D::pitch (  )  const [inline]

Get the PITCH angle (in radians).

See also:
setFromValues

Definition at line 307 of file CPose3D.h.

void mrpt::poses::CPose3D::rebuildHomogeneousMatrix (  )  [protected]

Rebuild the homog matrix from x,y,z and the angles.

double mrpt::poses::CPose3D::roll (  )  const [inline]

Get the ROLL angle (in radians).

See also:
setFromValues

Definition at line 308 of file CPose3D.h.

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 homogeneous coordinates matrix.

See also:
getYawPitchRoll, setYawPitchRoll

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 homogeneous coordinates matrix.

See also:
getYawPitchRoll, setFromValues

Definition at line 293 of file CPose3D.h.

void mrpt::poses::CPose3D::sphericalCoordinates ( const CPoint3D 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::x ( const double  x_  )  [inline, virtual]

Set the X coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 271 of file CPose3D.h.

double mrpt::poses::CPose3D::x (  )  const [inline]

Get the X coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 267 of file CPose3D.h.

void mrpt::poses::CPose3D::x_incr ( const double  Ax  )  [inline, virtual]

Increment the X coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 275 of file CPose3D.h.

void mrpt::poses::CPose3D::y ( const double  y_  )  [inline, virtual]

Set the Y coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 272 of file CPose3D.h.

double mrpt::poses::CPose3D::y (  )  const [inline]

Get the Y coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 268 of file CPose3D.h.

void mrpt::poses::CPose3D::y_incr ( const double  Ay  )  [inline, virtual]

Increment the Y coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 276 of file CPose3D.h.

double mrpt::poses::CPose3D::yaw (  )  const [inline]

Get the YAW angle (in radians).

See also:
setFromValues

Definition at line 306 of file CPose3D.h.

void mrpt::poses::CPose3D::z ( const double  z_  )  [inline, virtual]

Set the Z coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 273 of file CPose3D.h.

double mrpt::poses::CPose3D::z (  )  const [inline]

Get the Z coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 269 of file CPose3D.h.

void mrpt::poses::CPose3D::z_incr ( const double  Az  )  [inline, virtual]

Increment the Z coordinate.

Reimplemented from mrpt::poses::CPoseOrPoint.

Definition at line 277 of file CPose3D.h.


Friends And Related Function Documentation

friend class CPoint [friend]

Definition at line 168 of file CPose3D.h.

friend class CPose [friend]

Definition at line 166 of file CPose3D.h.

friend class CPose2D [friend]

Definition at line 167 of file CPose3D.h.

std::ostream MRPTDLLIMPEXP& operator<< ( std::ostream &  o,
const CPose3D p 
) [friend]


Member Data Documentation

The homogeneous matrix.

Definition at line 176 of file CPose3D.h.

double mrpt::poses::CPose3D::m_pitch [protected]

Definition at line 175 of file CPose3D.h.

double mrpt::poses::CPose3D::m_roll [protected]

These variables are updated every time that the object homogeneous matrix is modified (construction, loading from values, pose composition, etc ).

Definition at line 175 of file CPose3D.h.

double mrpt::poses::CPose3D::m_yaw [protected]

Definition at line 175 of file CPose3D.h.




Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:32:05 EDT 2009