Main MRPT website > C++ reference
MRPT logo

CPose3D.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPOSE3D_H
00029 #define CPOSE3D_H
00030 
00031 #include <mrpt/poses/CPose.h>
00032 #include <mrpt/math/CMatrixFixedNumeric.h>
00033 #include <mrpt/math/CQuaternion.h>
00034 
00035 namespace mrpt
00036 {
00037 namespace poses
00038 {
00039         using namespace mrpt::math;
00040 
00041         class BASE_IMPEXP CPose3DQuat;
00042 
00043         DEFINE_SERIALIZABLE_PRE( CPose3D )
00044 
00045         /** A class used to store a 3D pose (a 3D translation + a rotation in 3D).
00046          *   The 6D transformation in SE(3) stored in this class is kept in two
00047          *   separate containers: a 3-array for the translation, and a 3x3 rotation matrix.
00048          *
00049          *  The 6D pose is parameterized as a 6-vector: [x y z yaw pitch roll]. Note however,
00050          *   that the yaw/pitch/roll angles are only computed (on-demand and transparently)
00051          *   when the user requests them. Normally, rotations are handled via the 3x3 rotation matrix only.
00052          *
00053          *  For a complete descriptionan of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
00054          *    to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki.
00055          *
00056          *  To change the individual components of the pose, use CPose3D::setFromValues. This class assures that the internal
00057          *   3x3 rotation matrix is always up-to-date with the "yaw pitch roll" members.
00058          *
00059          *  Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.
00060          *
00061          *  This class and CPose3DQuat are very similar, and they can be converted to the each other automatically via transformation constructors.
00062          *
00063          *  There are Lie algebra methods: \a exp and \a ln (see the methods for documentation).
00064          *
00065          *  <div align=center>
00066          *   <img src="CPose3D.gif">
00067          *  </div>
00068          *
00069          * \sa CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
00070          */
00071         class BASE_IMPEXP CPose3D : public CPose<CPose3D>, public mrpt::utils::CSerializable
00072         {
00073                 // This must be added to any CSerializable derived class:
00074                 DEFINE_SERIALIZABLE( CPose3D )
00075 
00076         public:
00077                 CArrayDouble<3>   m_coords; //!< The translation vector [x,y,z]
00078                 CMatrixDouble33   m_ROT;    //!< The 3x3 rotation matrix
00079 
00080         protected:
00081                 mutable bool    m_ypr_uptodate;                 //!< Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
00082                 mutable double  m_yaw, m_pitch, m_roll; //!< These variables are updated every time that the object rotation matrix is modified (construction, loading from values, pose composition, etc )
00083 
00084                 /** Rebuild the homog matrix from the angles. */
00085                 void  rebuildRotationMatrix();
00086 
00087                 /** Updates Yaw/pitch/roll members from the m_ROT  */
00088                 inline void updateYawPitchRoll() const { if (!m_ypr_uptodate) { m_ypr_uptodate=true; getYawPitchRoll( m_yaw, m_pitch, m_roll ); } }
00089 
00090          public:
00091                 /** @name Constructors
00092                     @{ */
00093 
00094                 /** Default constructor, with all the coordinates set to zero. */
00095                 CPose3D();
00096 
00097                 /** Constructor with initilization of the pose; (remember that angles are always given in radians!)  */
00098                 CPose3D(const double x,const double  y,const double  z,const double  yaw=0, const double  pitch=0, const double roll=0);
00099 
00100                 /** 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). */
00101                 explicit CPose3D(const math::CMatrixDouble &m);
00102 
00103                 /** Constructor from a 4x4 homogeneous matrix: */
00104                 explicit CPose3D(const math::CMatrixDouble44 &m);
00105 
00106                 /** Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a TPoint3D */
00107                 template <class MATRIX33,class VECTOR3>
00108                 inline CPose3D(const MATRIX33 &rot, const VECTOR3& xyz) : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
00109                 {
00110                         ASSERT_EQUAL_(size(rot,1),3); ASSERT_EQUAL_(size(rot,2),3);ASSERT_EQUAL_(xyz.size(),3)
00111                         for (int r=0;r<3;r++)
00112                                 for (int c=0;c<3;c++)
00113                                         m_ROT(r,c)=rot.get_unsafe(r,c);
00114                         for (int r=0;r<3;r++) m_coords[r]=xyz[r];
00115                 }
00116                 //! \overload
00117                 inline CPose3D(const CMatrixDouble33 &rot, const CArrayDouble<3>& xyz) : m_coords(xyz),m_ROT(rot), m_ypr_uptodate(false)
00118                 { }
00119 
00120                 /** Constructor from a CPose2D object.
00121                 */
00122                 CPose3D(const CPose2D &);
00123 
00124                 /** Constructor from a CPoint3D object.
00125                 */
00126                 CPose3D(const CPoint3D &);
00127 
00128                 /** Constructor from lightweight object.
00129                 */
00130                 CPose3D(const mrpt::math::TPose3D &);
00131 
00132                 /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
00133                 CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z );
00134 
00135                 /** Constructor from a CPose3DQuat. */
00136                 CPose3D(const CPose3DQuat &);
00137 
00138                 /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
00139                 inline CPose3D(TConstructorFlags_Poses constructor_dummy_param) : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false) { }
00140 
00141                 /** Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
00142                   *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
00143                   *  \sa setFrom12Vector, getAs12Vector
00144                   */
00145                 inline explicit CPose3D(const CArrayDouble<12> &vec12) : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false) {
00146                         setFrom12Vector(vec12);
00147                 }
00148 
00149                 /** @} */  // end Constructors
00150 
00151 
00152 
00153                 /** @name Access 3x3 rotation and 4x4 homogeneous matrices
00154                     @{ */
00155 
00156                 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
00157                   * \sa getInverseHomogeneousMatrix, getRotationMatrix
00158                   */
00159                 inline void  getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
00160                 {
00161                         out_HM.insertMatrix(0,0,m_ROT);
00162                         for (int i=0;i<3;i++) out_HM(i,3)=m_coords[i];
00163                         out_HM(3,0)=out_HM(3,1)=out_HM(3,2)=0.; out_HM(3,3)=1.;
00164                 }
00165 
00166                 inline CMatrixDouble44 getHomogeneousMatrixVal() const { CMatrixDouble44 M; getHomogeneousMatrix(M); return M;}
00167 
00168                 /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix  */
00169                 inline void getRotationMatrix( mrpt::math::CMatrixDouble33 & ROT ) const { ROT = m_ROT; }
00170                 //! \overload
00171                 inline const mrpt::math::CMatrixDouble33 & getRotationMatrix() const { return m_ROT; }
00172 
00173                 /** @} */  // end rot and HM
00174 
00175 
00176                 /** @name Pose-pose and pose-point compositions and operators
00177                     @{ */
00178 
00179                 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
00180                 inline CPose3D  operator + (const CPose3D& b) const
00181                 {
00182                         CPose3D   ret(UNINITIALIZED_POSE);
00183                         ret.composeFrom(*this,b);
00184                         return ret;
00185                 }
00186 
00187                 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
00188                 CPoint3D  operator + (const CPoint3D& b) const;
00189 
00190                 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
00191                 CPoint3D  operator + (const CPoint2D& b) const;
00192 
00193         /** 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. */
00194         void sphericalCoordinates(
00195             const TPoint3D &point,
00196             double &out_range,
00197             double &out_yaw,
00198             double &out_pitch ) const;
00199 
00200                 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
00201                   *  If pointers are provided, the corresponding Jacobians are returned.
00202                   *  "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3).
00203                   *  See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
00204                   *  \param  If set to true, the Jacobian "out_jacobian_df_dpose" uses a fastest linearized appoximation (valid only for small rotations!).
00205                   */
00206                 void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
00207                         mrpt::math::CMatrixFixedNumeric<double,3,3>  *out_jacobian_df_dpoint=NULL,
00208                         mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dpose=NULL,
00209                         mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dse3=NULL,
00210                         bool use_small_rot_approx = false) const;
00211 
00212                 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
00213                   * \note local_point is passed by value to allow global and local point to be the same variable
00214                   */
00215                 inline void composePoint(const TPoint3D local_point, TPoint3D &global_point) const {
00216                         composePoint(local_point.x,local_point.y,local_point.z,  global_point.x,global_point.y,global_point.z );
00217                 }
00218 
00219                 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.  */
00220                 inline void composePoint(double lx,double ly,double lz, float &gx, float &gy, float &gz ) const {
00221                         double ggx, ggy,ggz;
00222                         composePoint(lx,ly,lz,ggx,ggy,ggz);
00223                         gx = ggx; gy = ggy; gz = ggz;
00224                 }
00225 
00226                 /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
00227                   *  If pointers are provided, the corresponding Jacobians are returned.
00228                   *  "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3).
00229                   *  See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
00230                   * \sa composePoint, composeFrom
00231                   */
00232                 void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
00233                         mrpt::math::CMatrixFixedNumeric<double,3,3>  *out_jacobian_df_dpoint=NULL,
00234                         mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dpose=NULL,
00235                         mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dse3=NULL ) const;
00236 
00237                 /**  Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
00238                   *  \note A or B can be "this" without problems.
00239                   */
00240                 void composeFrom(const CPose3D& A, const CPose3D& B );
00241 
00242                 /** Make \f$ this = this \oplus b \f$  (\a b can be "this" without problems) */
00243                 inline CPose3D&  operator += (const CPose3D& b)
00244                 {
00245                         composeFrom(*this,b);
00246                         return *this;
00247                 }
00248 
00249                 /**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
00250                   *  \note A or B can be "this" without problems.
00251                   * \sa composeFrom, composePoint
00252                   */
00253                 void inverseComposeFrom(const CPose3D& A, const CPose3D& B );
00254 
00255                 /** Compute \f$ RET = this \oplus b \f$  */
00256                 inline CPose3D  operator - (const CPose3D& b) const
00257                 {
00258                         CPose3D ret(UNINITIALIZED_POSE);
00259                         ret.inverseComposeFrom(*this,b);
00260                         return ret;
00261                 }
00262 
00263                 /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
00264                 void inverse();
00265 
00266                 /** makes: this = p (+) this */
00267                 inline void  changeCoordinatesReference( const CPose3D & p ) { composeFrom(p,CPose3D(*this)); }
00268 
00269                 /** @} */  // compositions
00270 
00271 
00272                 /** @name Access and modify contents
00273                         @{ */
00274 
00275                 /** Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators.
00276                   * \sa normalizeAngles
00277                   */
00278                 void addComponents(const CPose3D &p);
00279 
00280                 /** Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)
00281                   * \sa addComponents
00282                   */
00283                 void  normalizeAngles();
00284 
00285                 /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval). */
00286                 void operator *=(const double s);
00287 
00288                 /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.
00289                   * \sa getYawPitchRoll, setYawPitchRoll
00290                   */
00291                 void  setFromValues(
00292                         const double            x0,
00293                         const double            y0,
00294                         const double            z0,
00295                         const double            yaw=0,
00296                         const double            pitch=0,
00297                         const double            roll=0);
00298 
00299                 /** 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.
00300                   * \sa setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion, getAsQuaternion
00301                   */
00302                 template <typename VECTORLIKE>
00303                 inline void  setFromXYZQ(
00304                         const VECTORLIKE    &v,
00305                         const size_t        index_offset = 0)
00306                 {
00307                         ASSERT_ABOVEEQ_(v.size(), 7+index_offset)
00308                         // The 3x3 rotation part:
00309                         mrpt::math::CQuaternion<typename VECTORLIKE::value_type> q( v[index_offset+3],v[index_offset+4],v[index_offset+5],v[index_offset+6] );
00310                         q.rotationMatrixNoResize(m_ROT);
00311                         m_ypr_uptodate=false;
00312                         m_coords[0] = v[index_offset+0];
00313                         m_coords[1] = v[index_offset+1];
00314                         m_coords[2] = v[index_offset+2];
00315                 }
00316 
00317                 /** Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix.
00318                   * \sa getYawPitchRoll, setFromValues
00319                   */
00320                 inline void  setYawPitchRoll(
00321                         const double            yaw_,
00322                         const double            pitch_,
00323                         const double            roll_)
00324                 {
00325                         setFromValues(x(),y(),z(),yaw_,pitch_,roll_);
00326                 }
00327 
00328                 /** Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
00329                   *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
00330                   *  \sa getAs12Vector
00331                   */
00332                 template <class ARRAYORVECTOR>
00333                 inline void setFrom12Vector(const ARRAYORVECTOR &vec12)
00334                 {
00335                         m_ROT.set_unsafe(0,0, vec12[0]); m_ROT.set_unsafe(0,1, vec12[3]); m_ROT.set_unsafe(0,2, vec12[6]);
00336                         m_ROT.set_unsafe(1,0, vec12[1]); m_ROT.set_unsafe(1,1, vec12[4]); m_ROT.set_unsafe(1,2, vec12[7]);
00337                         m_ROT.set_unsafe(2,0, vec12[2]); m_ROT.set_unsafe(2,1, vec12[5]); m_ROT.set_unsafe(2,2, vec12[8]);
00338                         m_coords[0] = vec12[ 9];
00339                         m_coords[1] = vec12[10];
00340                         m_coords[2] = vec12[11];
00341                 }
00342 
00343                 /** Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
00344                   *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
00345                   *  \sa setFrom12Vector
00346                   */
00347                 template <class ARRAYORVECTOR>
00348                 inline void getAs12Vector(ARRAYORVECTOR &vec12) const
00349                 {
00350                         vec12[0] = m_ROT.get_unsafe(0,0); vec12[3] = m_ROT.get_unsafe(0,1); vec12[6] = m_ROT.get_unsafe(0,2);
00351                         vec12[1] = m_ROT.get_unsafe(1,0); vec12[4] = m_ROT.get_unsafe(1,1); vec12[7] = m_ROT.get_unsafe(1,2);
00352                         vec12[2] = m_ROT.get_unsafe(2,0); vec12[5] = m_ROT.get_unsafe(2,1); vec12[8] = m_ROT.get_unsafe(2,2);
00353                         vec12[ 9] = m_coords[0];
00354                         vec12[10] = m_coords[1];
00355                         vec12[11] = m_coords[2];
00356                 }
00357 
00358                 /** Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
00359                   * \sa setFromValues, yaw, pitch, roll
00360                   */
00361                 void  getYawPitchRoll( double &yaw, double &pitch, double &roll ) const;
00362 
00363                 inline double yaw() const { updateYawPitchRoll(); return m_yaw; }  //!< Get the YAW angle (in radians)  \sa setFromValues
00364                 inline double pitch() const { updateYawPitchRoll(); return m_pitch; }  //!< Get the PITCH angle (in radians) \sa setFromValues
00365                 inline double roll() const { updateYawPitchRoll(); return m_roll; }  //!< Get the ROLL angle (in radians) \sa setFromValues
00366 
00367                 /** Returns a 1x6 vector with [x y z yaw pitch roll] */
00368                 void getAsVector(vector_double &v) const;
00369 
00370                 /** Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
00371                   * \f[ \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) \f]
00372                   * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
00373                   * \param 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).
00374                   */
00375                 void getAsQuaternion(
00376                         mrpt::math::CQuaternionDouble &q,
00377                         mrpt::math::CMatrixFixedNumeric<double,4,3>   *out_dq_dr = NULL
00378                         ) const;
00379 
00380                 inline const double &operator[](unsigned int i) const
00381                 {
00382                         updateYawPitchRoll();
00383                         switch(i)
00384                         {
00385                                 case 0:return m_coords[0];
00386                                 case 1:return m_coords[1];
00387                                 case 2:return m_coords[2];
00388                                 case 3:return m_yaw;
00389                                 case 4:return m_pitch;
00390                                 case 5:return m_roll;
00391                                 default:
00392                                 throw std::runtime_error("CPose3D::operator[]: Index of bounds.");
00393                         }
00394                 }
00395                 // CPose3D CANNOT have a write [] operator, since it'd leave the object in an inconsistent state (outdated rotation matrix).
00396                 // Use setFromValues() instead.
00397                 // inline double &operator[](unsigned int i)
00398 
00399                 /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
00400                   * \sa fromString
00401                   */
00402                 void asString(std::string &s) const { updateYawPitchRoll(); s = mrpt::format("[%f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],RAD2DEG(m_yaw),RAD2DEG(m_pitch),RAD2DEG(m_roll)); }
00403                 inline std::string asString() const { std::string s; asString(s); return s; }
00404 
00405                 /** Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg. )
00406                   * \sa asString
00407                   * \exception std::exception On invalid format
00408                   */
00409                 void fromString(const std::string &s) {
00410                         CMatrixDouble  m;
00411                         if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00412                         ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
00413                         this->setFromValues(m.get_unsafe(0,0),m.get_unsafe(0,1),m.get_unsafe(0,2),DEG2RAD(m.get_unsafe(0,3)),DEG2RAD(m.get_unsafe(0,4)),DEG2RAD(m.get_unsafe(0,5)));
00414                  }
00415 
00416                 /** 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). */
00417                 bool isHorizontal( const double tolerance=0) const;
00418 
00419                 /** The euclidean distance between two poses taken as two 6-length vectors (angles in radians). */
00420                 double distanceEuclidean6D( const CPose3D &o ) const;
00421 
00422                 /** @} */  // modif. components
00423 
00424 
00425 
00426                 /** @name Lie Algebra methods
00427                     @{ */
00428 
00429                 /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
00430                   * \note Method from TooN (C) Tom Drummond (GNU GPL) */
00431                 static CPose3D exp(const mrpt::math::CArrayNumeric<double,6> & vect);
00432 
00433                 /** Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix).
00434                   * \note Method from TooN (C) Tom Drummond (GNU GPL) */
00435                 static CMatrixDouble33 exp_rotation(const mrpt::math::CArrayNumeric<double,3> & vect);
00436 
00437 
00438                 /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra.
00439                   * \note Method from TooN (C) Tom Drummond (GNU GPL)
00440                   * \sa ln_jacob
00441                   */
00442                 void ln(mrpt::math::CArrayDouble<6> &out_ln) const;
00443 
00444                 /// \overload
00445                 inline mrpt::math::CArrayDouble<6> ln() const { mrpt::math::CArrayDouble<6> ret; ln(ret); return ret; }
00446 
00447                 /** Jacobian of the logarithm of the 3x4 matrix defined by this pose.
00448                   * \note Method from TooN (C) Tom Drummond (GNU GPL)
00449                   * \sa ln
00450                   */
00451                 void ln_jacob(mrpt::math::CMatrixFixedNumeric<double,6,12> &J) const;
00452 
00453                 /** Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rotation matrix R.
00454                   * \sa ln, ln_jacob
00455                   */
00456                 static void ln_rot_jacob(const CMatrixDouble33 &R, CMatrixFixedNumeric<double,3,9> &M);
00457 
00458                 /** Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra.
00459                   * \note Method from TooN (C) Tom Drummond (GNU GPL) */
00460                 CArrayDouble<3> ln_rotation() const;
00461 
00462                 /** @} */
00463 
00464                 typedef CPose3D  type_value; //!< Used to emulate CPosePDF types, for example, in CNetworkOfPoses
00465                 enum { is_3D_val = 1 };
00466                 static inline bool is_3D() { return is_3D_val!=0; }
00467                 enum { rotation_dimensions = 3 };
00468                 enum { is_PDF_val = 0 };
00469                 static inline bool is_PDF() { return is_PDF_val!=0; }
00470 
00471                 /** @name STL-like methods and typedefs
00472                    @{   */
00473                 typedef double         value_type;              //!< The type of the elements
00474                 typedef double&        reference;
00475                 typedef const double&  const_reference;
00476                 typedef std::size_t    size_type;
00477                 typedef std::ptrdiff_t difference_type;
00478 
00479 
00480                 // size is constant
00481                 enum { static_size = 6 };
00482                 static inline size_type size() { return static_size; }
00483                 static inline bool empty() { return false; }
00484                 static inline size_type max_size() { return static_size; }
00485                 static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3D to %u.",static_cast<unsigned>(n))); }
00486                 /** @} */
00487 
00488         }; // End of class def.
00489 
00490 
00491         std::ostream BASE_IMPEXP  & operator << (std::ostream& o, const CPose3D& p);
00492 
00493         /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */
00494         CPose3D BASE_IMPEXP operator -(const CPose3D &p);
00495 
00496         bool BASE_IMPEXP operator==(const CPose3D &p1,const CPose3D &p2);
00497         bool BASE_IMPEXP operator!=(const CPose3D &p1,const CPose3D &p2);
00498 
00499 
00500         // This is a member of CPose<>, but has to be defined here since in its header CPose3D is not declared yet.
00501         /** The operator \f$ a \ominus b \f$ is the pose inverse compounding operator. */
00502         template <class DERIVEDCLASS> CPose3D CPose<DERIVEDCLASS>::operator -(const CPose3D& b) const
00503         {
00504                 CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
00505                 b.getInverseHomogeneousMatrix( B_INV );
00506                 CMatrixDouble44 HM(UNINITIALIZED_MATRIX);
00507                 static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(HM);
00508                 CMatrixDouble44 RES(UNINITIALIZED_MATRIX);
00509                 RES.multiply(B_INV,HM);
00510                 return CPose3D( RES );
00511         }
00512 
00513 
00514         } // End of namespace
00515 } // End of namespace
00516 
00517 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011