00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3D, CPose ) 00040 00041 /** A class used to store a 3D pose. 00042 * A class used to store a 3D (6D) pose, including the 3D coordinate 00043 * point and orientation angles. It is used in many situations, 00044 * from defining a robot pose, maps relative poses, sensors,... 00045 * See introduction in documentation for the CPoseOrPoint class. 00046 <br>For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint<br> 00047 * 00048 * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer 00049 * to the <a href="http://babel.isa.uma.es/mrpt/index.php/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki. 00050 * 00051 * To change the individual components of the pose, use CPose3D::setFromValues. This class assures that the internal 00052 * 4x4 homogeneous coordinate matrix is always up-to-date with the "x y z yaw pitch roll" members. 00053 * 00054 * Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion. 00055 00056 <div align=center> 00057 00058 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00059 style='border-collapse:collapse;border:none'> 00060 <tr style='height:15.8pt'> 00061 <td width=676 colspan=2 style='width:507.25pt;border:solid windowtext 1.0pt; 00062 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00063 <p align=center style='text-align:center'>poses::CPose3D</p> 00064 </td> 00065 </tr> 00066 <tr style='height:15.8pt'> 00067 <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top: 00068 none;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00069 <p align=center style='text-align:center'>Homogeneous 00070 transfomation matrix</p> 00071 </td> 00072 <td width=326 style='width:244.6pt;border-top:none;border-left:none; 00073 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00074 padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00075 <p align=center style='text-align:center'>Spatial 00076 representation</p> 00077 </td> 00078 </tr> 00079 <tr style='height:202.65pt'> 00080 <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top: 00081 none;padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'> 00082 <div align=center> 00083 <table Table border=0 cellspacing=0 cellpadding=0 width=334 00084 style='width:250.65pt;border-collapse:collapse'> 00085 <tr style='height:16.65pt'> 00086 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00087 <p align=center style='text-align:center'>cycp</p> 00088 </td> 00089 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00090 <p align=center style='text-align:center'>cyspsr-sycr</p> 00091 </td> 00092 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00093 <p align=center style='text-align:center'>cyspcr+sysr</p> 00094 </td> 00095 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00096 <p align=center style='text-align:center'>x</p> 00097 </td> 00098 </tr> 00099 <tr style='height:17.25pt'> 00100 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00101 <p align=center style='text-align:center'>sycp</p> 00102 </td> 00103 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00104 <p align=center style='text-align:center'>syspsr+cycr</p> 00105 </td> 00106 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00107 <p align=center style='text-align:center'>syspcr-cysr</p> 00108 </td> 00109 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00110 <p align=center style='text-align:center'>y</p> 00111 </td> 00112 </tr> 00113 <tr style='height:19.65pt'> 00114 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00115 <p align=center style='text-align:center'>-sp</p> 00116 </td> 00117 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00118 <p align=center style='text-align:center'>cpsr</p> 00119 </td> 00120 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00121 <p align=center style='text-align:center'>cpcr</p> 00122 </td> 00123 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00124 <p align=center style='text-align:center'>z</p> 00125 </td> 00126 </tr> 00127 <tr style='height:11.0pt'> 00128 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00129 <p align=center style='text-align:center'>0</p> 00130 </td> 00131 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00132 <p align=center style='text-align:center'>0</p> 00133 </td> 00134 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00135 <p align=center style='text-align:center'>0</p> 00136 </td> 00137 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00138 <p align=center style='text-align:center'>1</p> 00139 </td> 00140 </tr> 00141 </table> 00142 </div> 00143 <p align=center style='text-align:center'><span lang=EN-GB>where:</span></p> 00144 <p align=center style='text-align:center'><span lang=EN-GB>cy 00145 = cos Yaw ; sy = sin Yaw</span></p> 00146 <p align=center style='text-align:center'><span lang=EN-GB>cp 00147 = cos Pitch ; sp = sin Pitch</span></p> 00148 <p align=center style='text-align:center'><span lang=EN-GB>cr 00149 = cos Roll ; sr = sin Roll</span></p> 00150 </td> 00151 <td width=326 style='width:244.6pt;border-top:none;border-left:none; 00152 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00153 padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'> 00154 <p align=center style='text-align:center'><span lang=EN-GB><img src="CPose3D.gif"></span></p> 00155 </td> 00156 </tr> 00157 </table> 00158 00159 </div> 00160 00161 * 00162 * \sa CPoseOrPoint,CPoint3D 00163 */ 00164 class MRPTDLLIMPEXP CPose3D : public CPose 00165 { 00166 friend class CPose; 00167 friend class CPose2D; 00168 friend class CPoint; 00169 friend std::ostream MRPTDLLIMPEXP & operator << (std::ostream& o, const CPose3D& p); 00170 00171 // This must be added to any CSerializable derived class: 00172 DEFINE_SERIALIZABLE( CPose3D ) 00173 00174 protected: 00175 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 ) 00176 mutable CMatrixDouble44 m_HM; //!< The homogeneous matrix 00177 00178 /** Rebuild the homog matrix from x,y,z and the angles. */ 00179 void rebuildHomogeneousMatrix(); 00180 00181 public: 00182 /** Constructor with initilization of the pose; (remember that angles are always given in radians!) 00183 */ 00184 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); 00185 00186 /** Copy constructor. 00187 */ 00188 CPose3D( const CPose3D &o); 00189 00190 /** Copy operator. 00191 */ 00192 CPose3D & operator=( const CPose3D &o); 00193 00194 /** Constructor from a 4x4 homogeneous matrix: */ 00195 explicit CPose3D(const math::CMatrixDouble &m); 00196 00197 /** Constructor from a 4x4 homogeneous matrix: */ 00198 explicit CPose3D(const math::CMatrixDouble44 &m); 00199 00200 /** Constructor from a CPose2D object. 00201 */ 00202 CPose3D(const CPose2D &); 00203 00204 /** Constructor from a CPoint3D object. 00205 */ 00206 CPose3D(const CPoint3D &); 00207 00208 /** Constructor from lightweight object. 00209 */ 00210 CPose3D(const mrpt::math::TPose3D &); 00211 00212 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). 00213 * \sa getInverseHomogeneousMatrix 00214 */ 00215 void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const { 00216 out_HM = m_HM; 00217 } 00218 00219 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. 00220 */ 00221 CPose3D operator + (const CPose3D& b) const; 00222 00223 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. 00224 */ 00225 CPoint3D operator + (const CPoint3D& b) const; 00226 00227 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. 00228 */ 00229 CPoint3D operator + (const CPoint2D& b) const; 00230 00231 /** Scalar sum of components: This is diferent from poses 00232 * composition, which is implemented as "+" operators. 00233 * \sa normalizeAngles 00234 */ 00235 void addComponents(const CPose3D &p); 00236 00237 /** Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents) 00238 * \sa addComponents 00239 */ 00240 void normalizeAngles(); 00241 00242 /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval). 00243 */ 00244 void operator *=(const double & s); 00245 00246 /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. 00247 * For the coordinate system see the top of this page. 00248 */ 00249 void sphericalCoordinates( 00250 const CPoint3D &point, 00251 double &out_range, 00252 double &out_yaw, 00253 double &out_pitch ) const; 00254 00255 /** 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. */ 00256 void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz) const; 00257 00258 /** 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. */ 00259 void composePoint(float lx,float ly,float lz, float &gx, float &gy, float &gz) const; 00260 00261 /** 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. 00262 * \note local_point is passed by value to allow global and local point to be the same variable 00263 */ 00264 void composePoint(const TPoint3D local_point, TPoint3D &global_point) const; 00265 00266 // These three must be declared here because the next three virtual ones hide the base methods. 00267 double x() const { return m_x; } //!< Get the X coordinate 00268 double y() const { return m_y; } //!< Get the Y coordinate 00269 double z() const { return m_z; } //!< Get the Z coordinate 00270 00271 void x(const double x_) { m_HM.get_unsafe(0,3)= m_x=x_; } //!< Set the X coordinate 00272 void y(const double y_) { m_HM.get_unsafe(1,3)= m_y=y_; } //!< Set the Y coordinate 00273 void z(const double z_) { m_HM.get_unsafe(2,3)= m_z=z_; } //!< Set the Z coordinate 00274 00275 void x_incr(const double Ax) { m_HM.get_unsafe(0,3)= m_x+=Ax; } //!< Increment the X coordinate 00276 void y_incr(const double Ay) { m_HM.get_unsafe(1,3)= m_y+=Ay; } //!< Increment the Y coordinate 00277 void z_incr(const double Az) { m_HM.get_unsafe(2,3)= m_z+=Az; } //!< Increment the Z coordinate 00278 00279 /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal homogeneous coordinates matrix. 00280 * \sa getYawPitchRoll, setYawPitchRoll 00281 */ 00282 void setFromValues( 00283 const double x0, 00284 const double y0, 00285 const double z0, 00286 const double yaw=0, 00287 const double pitch=0, 00288 const double roll=0); 00289 00290 /** Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal homogeneous coordinates matrix. 00291 * \sa getYawPitchRoll, setFromValues 00292 */ 00293 void setYawPitchRoll( 00294 const double yaw_, 00295 const double pitch_, 00296 const double roll_) 00297 { 00298 setFromValues(x(),y(),z(),yaw_,pitch_,roll_); 00299 } 00300 00301 /** Returns the three angles (yaw, pitch, roll), in radians, from the homogeneous matrix. 00302 * \sa setFromValues, yaw, pitch, roll 00303 */ 00304 void getYawPitchRoll( double &yaw, double &pitch, double &roll ); 00305 00306 double yaw() const { return m_yaw; } //!< Get the YAW angle (in radians) \sa setFromValues 00307 double pitch() const { return m_pitch; } //!< Get the PITCH angle (in radians) \sa setFromValues 00308 double roll() const { return m_roll; } //!< Get the ROLL angle (in radians) \sa setFromValues 00309 00310 /** The euclidean distance between two poses taken as two 6-length vectors (angles in radians). 00311 */ 00312 double distanceEuclidean6D( const CPose3D &o ) const; 00313 00314 /** Returns a 1x6 vector with [x y z yaw pitch roll] */ 00315 void getAsVector(vector_double &v) const; 00316 00317 /** Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) 00318 * \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] 00319 * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$. 00320 */ 00321 void getAsQuaternion(mrpt::math::CQuaternionDouble &q) const; 00322 00323 00324 /** Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object. 00325 * \note A or B can be "this" without problems. 00326 */ 00327 void composeFrom(const CPose3D& A, const CPose3D& B ); 00328 00329 /** Make \f$ this = this \oplus b \f$ */ 00330 CPose3D& operator += (const CPose3D& b) 00331 { 00332 composeFrom(*this,b); 00333 return *this; 00334 } 00335 00336 }; // End of class def. 00337 00338 00339 std::ostream MRPTDLLIMPEXP & operator << (std::ostream& o, const CPose3D& p); 00340 00341 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */ 00342 CPose3D MRPTDLLIMPEXP operator -(const CPose3D &p); 00343 00344 bool MRPTDLLIMPEXP operator==(const CPose3D &p1,const CPose3D &p2); 00345 bool MRPTDLLIMPEXP operator!=(const CPose3D &p1,const CPose3D &p2); 00346 00347 } // End of namespace 00348 } // End of namespace 00349 00350 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:02:22 EDT 2009 |