00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 class BASE_IMPEXP CPose3DQuat; 00040 00041 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3D, CPose ) 00042 00043 /** A class used to store a 3D pose. 00044 * A class used to store a 3D (6D) pose, including the 3D coordinate 00045 * point and orientation angles. It is used in many situations, 00046 * from defining a robot pose, maps relative poses, sensors,... 00047 * See introduction in documentation for the CPoseOrPoint class. 00048 <br>For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint<br> 00049 * 00050 * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer 00051 * to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki. 00052 * 00053 * To change the individual components of the pose, use CPose3D::setFromValues. This class assures that the internal 00054 * 4x4 homogeneous coordinate matrix is always up-to-date with the "x y z yaw pitch roll" members. 00055 * 00056 * Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion. 00057 * 00058 * This class and CPose3DQuat are very similar, and they can be converted to the each other automatically via transformation constructors. 00059 * 00060 00061 <div align=center> 00062 00063 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00064 style='border-collapse:collapse;border:none'> 00065 <tr style='height:15.8pt'> 00066 <td width=676 colspan=2 style='width:507.25pt;border:solid windowtext 1.0pt; 00067 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00068 <p align=center style='text-align:center'>poses::CPose3D</p> 00069 </td> 00070 </tr> 00071 <tr style='height:15.8pt'> 00072 <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top: 00073 none;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00074 <p align=center style='text-align:center'>Homogeneous 00075 transfomation matrix</p> 00076 </td> 00077 <td width=326 style='width:244.6pt;border-top:none;border-left:none; 00078 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00079 padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00080 <p align=center style='text-align:center'>Spatial 00081 representation</p> 00082 </td> 00083 </tr> 00084 <tr style='height:202.65pt'> 00085 <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top: 00086 none;padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'> 00087 <div align=center> 00088 <table Table border=0 cellspacing=0 cellpadding=0 width=334 00089 style='width:250.65pt;border-collapse:collapse'> 00090 <tr style='height:16.65pt'> 00091 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00092 <p align=center style='text-align:center'>cycp</p> 00093 </td> 00094 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00095 <p align=center style='text-align:center'>cyspsr-sycr</p> 00096 </td> 00097 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00098 <p align=center style='text-align:center'>cyspcr+sysr</p> 00099 </td> 00100 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00101 <p align=center style='text-align:center'>x</p> 00102 </td> 00103 </tr> 00104 <tr style='height:17.25pt'> 00105 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00106 <p align=center style='text-align:center'>sycp</p> 00107 </td> 00108 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00109 <p align=center style='text-align:center'>syspsr+cycr</p> 00110 </td> 00111 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00112 <p align=center style='text-align:center'>syspcr-cysr</p> 00113 </td> 00114 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00115 <p align=center style='text-align:center'>y</p> 00116 </td> 00117 </tr> 00118 <tr style='height:19.65pt'> 00119 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00120 <p align=center style='text-align:center'>-sp</p> 00121 </td> 00122 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00123 <p align=center style='text-align:center'>cpsr</p> 00124 </td> 00125 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00126 <p align=center style='text-align:center'>cpcr</p> 00127 </td> 00128 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00129 <p align=center style='text-align:center'>z</p> 00130 </td> 00131 </tr> 00132 <tr style='height:11.0pt'> 00133 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00134 <p align=center style='text-align:center'>0</p> 00135 </td> 00136 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00137 <p align=center style='text-align:center'>0</p> 00138 </td> 00139 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00140 <p align=center style='text-align:center'>0</p> 00141 </td> 00142 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00143 <p align=center style='text-align:center'>1</p> 00144 </td> 00145 </tr> 00146 </table> 00147 </div> 00148 <p align=center style='text-align:center'><span lang=EN-GB>where:</span></p> 00149 <p align=center style='text-align:center'><span lang=EN-GB>cy 00150 = cos Yaw ; sy = sin Yaw</span></p> 00151 <p align=center style='text-align:center'><span lang=EN-GB>cp 00152 = cos Pitch ; sp = sin Pitch</span></p> 00153 <p align=center style='text-align:center'><span lang=EN-GB>cr 00154 = cos Roll ; sr = sin Roll</span></p> 00155 </td> 00156 <td width=326 style='width:244.6pt;border-top:none;border-left:none; 00157 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00158 padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'> 00159 <p align=center style='text-align:center'><span lang=EN-GB><img src="CPose3D.gif"></span></p> 00160 </td> 00161 </tr> 00162 </table> 00163 00164 </div> 00165 00166 * 00167 * \sa CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion 00168 */ 00169 class BASE_IMPEXP CPose3D : public CPose 00170 { 00171 friend class CPose; 00172 friend class CPose2D; 00173 friend class CPoint; 00174 friend std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3D& p); 00175 00176 // This must be added to any CSerializable derived class: 00177 DEFINE_SERIALIZABLE( CPose3D ) 00178 00179 protected: 00180 double m_yaw, m_pitch, m_roll; //!< These variables are updated every time that the object homogeneous matrix is modified (construction, loading from values, pose composition, etc ) 00181 mutable CMatrixDouble44 m_HM; //!< The homogeneous matrix 00182 00183 /** Rebuild the homog matrix from x,y,z and the angles. */ 00184 void rebuildHomogeneousMatrix(); 00185 00186 public: 00187 /** Default constructor, with all the coordinates to zero. */ 00188 CPose3D(); 00189 00190 /** Constructor with initilization of the pose; (remember that angles are always given in radians!) */ 00191 CPose3D(const double x,const double y,const double z,const double yaw=0, const double pitch=0, const double roll=0); 00192 00193 /** Copy constructor. 00194 */ 00195 CPose3D( const CPose3D &o); 00196 00197 /** Copy operator. 00198 */ 00199 CPose3D & operator=( const CPose3D &o); 00200 00201 /** Constructor from a 4x4 homogeneous matrix: */ 00202 explicit CPose3D(const math::CMatrixDouble &m); 00203 00204 /** Constructor from a 4x4 homogeneous matrix: */ 00205 explicit CPose3D(const math::CMatrixDouble44 &m); 00206 00207 /** Constructor from a CPose2D object. 00208 */ 00209 CPose3D(const CPose2D &); 00210 00211 /** Constructor from a CPoint3D object. 00212 */ 00213 CPose3D(const CPoint3D &); 00214 00215 /** Constructor from lightweight object. 00216 */ 00217 CPose3D(const mrpt::math::TPose3D &); 00218 00219 /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */ 00220 CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z ); 00221 00222 /** Constructor from a CPose3DQuat. */ 00223 CPose3D(const CPose3DQuat &); 00224 00225 /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */ 00226 inline CPose3D(bool,bool) : m_HM(UNINITIALIZED_MATRIX) { m_is3D = true; } 00227 00228 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). 00229 * \sa getInverseHomogeneousMatrix 00230 */ 00231 void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const { out_HM = m_HM; } 00232 00233 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. 00234 */ 00235 CPose3D operator + (const CPose3D& b) const; 00236 00237 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. 00238 */ 00239 CPoint3D operator + (const CPoint3D& b) const; 00240 00241 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. 00242 */ 00243 CPoint3D operator + (const CPoint2D& b) const; 00244 00245 /** Scalar sum of components: This is diferent from poses 00246 * composition, which is implemented as "+" operators. 00247 * \sa normalizeAngles 00248 */ 00249 void addComponents(const CPose3D &p); 00250 00251 /** Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents) 00252 * \sa addComponents 00253 */ 00254 void normalizeAngles(); 00255 00256 /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval). 00257 */ 00258 void operator *=(const double s); 00259 00260 /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. 00261 * For the coordinate system see the top of this page. 00262 */ 00263 void sphericalCoordinates( 00264 const TPoint3D &point, 00265 double &out_range, 00266 double &out_yaw, 00267 double &out_pitch ) const; 00268 00269 /** 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. */ 00270 void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz, 00271 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL, 00272 mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL) const; 00273 00274 /** 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. 00275 * \note local_point is passed by value to allow global and local point to be the same variable 00276 */ 00277 inline void composePoint(const TPoint3D local_point, TPoint3D &global_point) const { 00278 composePoint(local_point.x,local_point.y,local_point.z, global_point.x,global_point.y,global_point.z ); 00279 } 00280 00281 /** 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. */ 00282 inline void composePoint(double lx,double ly,double lz, float &gx, float &gy, float &gz ) const { 00283 double ggx, ggy,ggz; 00284 composePoint(lx,ly,lz,ggx,ggy,ggz); 00285 gx = ggx; gy = ggy; gz = ggz; 00286 } 00287 00288 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. 00289 * \sa composePoint, composeFrom 00290 */ 00291 void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz, 00292 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL, 00293 mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL) const; 00294 00295 /** Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object. 00296 * \note A or B can be "this" without problems. 00297 */ 00298 void composeFrom(const CPose3D& A, const CPose3D& B ); 00299 00300 /** Make \f$ this = this \oplus b \f$ */ 00301 inline CPose3D& operator += (const CPose3D& b) 00302 { 00303 composeFrom(*this,b); 00304 return *this; 00305 } 00306 00307 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object. 00308 * \note A or B can be "this" without problems. 00309 * \sa composeFrom, composePoint 00310 */ 00311 void inverseComposeFrom(const CPose3D& A, const CPose3D& B ); 00312 00313 /** Compute \f$ RET = this \oplus b \f$ */ 00314 inline CPose3D operator - (const CPose3D& b) const 00315 { 00316 CPose3D ret(UNINITIALIZED_POSE); 00317 ret.inverseComposeFrom(*this,b); 00318 return ret; 00319 } 00320 00321 00322 // These three must be declared here because the next three virtual ones hide the base methods. 00323 inline double x() const { return m_x; } //!< Get the X coordinate 00324 inline double y() const { return m_y; } //!< Get the Y coordinate 00325 inline double z() const { return m_z; } //!< Get the Z coordinate 00326 00327 void x(const double x_) { m_HM.get_unsafe(0,3)= m_x=x_; } //!< Set the X coordinate 00328 void y(const double y_) { m_HM.get_unsafe(1,3)= m_y=y_; } //!< Set the Y coordinate 00329 void z(const double z_) { m_HM.get_unsafe(2,3)= m_z=z_; } //!< Set the Z coordinate 00330 00331 inline void x_incr(const double Ax) { m_HM.get_unsafe(0,3)= m_x+=Ax; } //!< Increment the X coordinate 00332 inline void y_incr(const double Ay) { m_HM.get_unsafe(1,3)= m_y+=Ay; } //!< Increment the Y coordinate 00333 inline void z_incr(const double Az) { m_HM.get_unsafe(2,3)= m_z+=Az; } //!< Increment the Z coordinate 00334 00335 /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal homogeneous coordinates matrix. 00336 * \sa getYawPitchRoll, setYawPitchRoll 00337 */ 00338 void setFromValues( 00339 const double x0, 00340 const double y0, 00341 const double z0, 00342 const double yaw=0, 00343 const double pitch=0, 00344 const double roll=0); 00345 00346 /** 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. 00347 * \sa setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion, getAsQuaternion 00348 */ 00349 template <typename VECTORLIKE> 00350 inline void setFromXYZQ( 00351 const VECTORLIKE &v, 00352 const size_t index_offset = 0) 00353 { 00354 ASSERT_(v.size()>=7) 00355 // The 3x3 rotation part: 00356 mrpt::math::CQuaternion<typename VECTORLIKE::value_type> q( v[index_offset+3],v[index_offset+4],v[index_offset+5],v[index_offset+6] ); 00357 q.rotationMatrixNoResize(m_HM); 00358 getYawPitchRoll( m_yaw, m_pitch, m_roll ); 00359 m_HM.get_unsafe(0,3)=m_x = v[index_offset+0]; 00360 m_HM.get_unsafe(1,3)=m_y = v[index_offset+1]; 00361 m_HM.get_unsafe(2,3)=m_z = v[index_offset+2]; 00362 m_HM.get_unsafe(3,0)=m_HM.get_unsafe(3,1)=m_HM.get_unsafe(3,2)=0; 00363 m_HM.get_unsafe(3,3)=1.0; 00364 } 00365 00366 /** Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal homogeneous coordinates matrix. 00367 * \sa getYawPitchRoll, setFromValues 00368 */ 00369 inline void setYawPitchRoll( 00370 const double yaw_, 00371 const double pitch_, 00372 const double roll_) 00373 { 00374 setFromValues(x(),y(),z(),yaw_,pitch_,roll_); 00375 } 00376 00377 /** Returns the three angles (yaw, pitch, roll), in radians, from the homogeneous matrix. 00378 * \sa setFromValues, yaw, pitch, roll 00379 */ 00380 void getYawPitchRoll( double &yaw, double &pitch, double &roll ); 00381 00382 inline double yaw() const { return m_yaw; } //!< Get the YAW angle (in radians) \sa setFromValues 00383 inline double pitch() const { return m_pitch; } //!< Get the PITCH angle (in radians) \sa setFromValues 00384 inline double roll() const { return m_roll; } //!< Get the ROLL angle (in radians) \sa setFromValues 00385 00386 /** The euclidean distance between two poses taken as two 6-length vectors (angles in radians). 00387 */ 00388 double distanceEuclidean6D( const CPose3D &o ) const; 00389 00390 /** Returns a 1x6 vector with [x y z yaw pitch roll] */ 00391 void getAsVector(vector_double &v) const; 00392 00393 /** Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) 00394 * \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] 00395 * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$. 00396 * \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). 00397 */ 00398 void getAsQuaternion( 00399 mrpt::math::CQuaternionDouble &q, 00400 mrpt::math::CMatrixFixedNumeric<double,4,3> *out_dq_dr = NULL 00401 ) const; 00402 00403 00404 /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.) 00405 * \sa fromString 00406 */ 00407 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f]",m_x,m_y,m_z,RAD2DEG(m_yaw),RAD2DEG(m_pitch),RAD2DEG(m_roll)); } 00408 inline std::string asString() const { std::string s; asString(s); return s; } 00409 00410 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" ) 00411 * \sa asString 00412 * \exception std::exception On invalid format 00413 */ 00414 void fromString(const std::string &s) { 00415 CMatrixDouble m; 00416 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); 00417 ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString"); 00418 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))); 00419 } 00420 00421 /** 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). 00422 */ 00423 bool isHorizontal( const double tolerance=0) const; 00424 00425 inline const double &operator[](unsigned int i) const 00426 { 00427 switch(i) 00428 { 00429 case 0:return m_x; 00430 case 1:return m_y; 00431 case 2:return m_z; 00432 case 3:return m_yaw; 00433 case 4:return m_pitch; 00434 case 5:return m_roll; 00435 default: 00436 throw std::runtime_error("CPose3D::operator[]: Index of bounds."); 00437 } 00438 } 00439 // CPose3D CANNOT have a write [] operator, since it'd leave the object in an inconsistent state. 00440 // Use setFromValues(). 00441 // inline double &operator[](unsigned int i) 00442 00443 /** @name STL-like methods and typedefs 00444 @{ */ 00445 typedef double value_type; //!< The type of the elements 00446 typedef double& reference; 00447 typedef const double& const_reference; 00448 typedef std::size_t size_type; 00449 typedef std::ptrdiff_t difference_type; 00450 00451 00452 // size is constant 00453 enum { static_size = 6 }; 00454 static inline size_type size() { return static_size; } 00455 static inline bool empty() { return false; } 00456 static inline size_type max_size() { return static_size; } 00457 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))); } 00458 /** @} */ 00459 00460 }; // End of class def. 00461 00462 00463 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3D& p); 00464 00465 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */ 00466 CPose3D BASE_IMPEXP operator -(const CPose3D &p); 00467 00468 bool BASE_IMPEXP operator==(const CPose3D &p1,const CPose3D &p2); 00469 bool BASE_IMPEXP operator!=(const CPose3D &p1,const CPose3D &p2); 00470 00471 } // End of namespace 00472 } // End of namespace 00473 00474 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
