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 CPOSE2D_H 00029 #define CPOSE2D_H 00030 00031 #include <mrpt/poses/CPose.h> 00032 00033 namespace mrpt 00034 { 00035 namespace poses 00036 { 00037 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose2D, CPose ) 00038 00039 /** A class used to store a 2D pose. 00040 * A class used to store a 2D pose, including the 2D coordinate 00041 * point and a heading (phi) angle. It is used in many situations, 00042 * from defining a robot pose, maps relative poses, sensors,... 00043 * 00044 * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer 00045 * to the <a href="http://babel.isa.uma.es/mrpt/index.php/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki. 00046 * 00047 00048 <div align=center> 00049 00050 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00051 style='border-collapse:collapse;border:none'> 00052 <tr> 00053 <td width=576 colspan=2 style='width:432.2pt;border:solid windowtext 1.0pt; 00054 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt'> 00055 <p align=center style='text-align:center'>poses::CPose2D</p> 00056 </td> 00057 </tr> 00058 <tr> 00059 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00060 none;padding:0cm 5.4pt 0cm 5.4pt'> 00061 <p align=center style='text-align:center'>Homogeneous 00062 transfomation matrix</p> 00063 </td> 00064 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00065 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00066 padding:0cm 5.4pt 0cm 5.4pt'> 00067 <p align=center style='text-align:center'>Spatial 00068 representation</p> 00069 </td> 00070 </tr> 00071 <tr style='height:108.3pt'> 00072 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00073 none;padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00074 <div align=center> 00075 <table Table border=0 cellspacing=0 cellpadding=0 width="67%" 00076 style='width:67.92%;border-collapse:collapse'> 00077 <tr style='height:20.6pt'> 00078 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00079 <p align=center style='text-align:center'>cos<span 00080 style='font-family:Symbol'>j</span></p> 00081 </td> 00082 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00083 <p align=center style='text-align:center'>-sin<span 00084 style='font-family:Symbol'>j</span></p> 00085 </td> 00086 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00087 <p align=center style='text-align:center'>0</p> 00088 </td> 00089 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00090 <p align=center style='text-align:center'>x</p> 00091 </td> 00092 </tr> 00093 <tr style='height:20.6pt'> 00094 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00095 <p align=center style='text-align:center'>sin<span 00096 style='font-family:Symbol'>j</span></p> 00097 </td> 00098 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00099 <p align=center style='text-align:center'>cos<span 00100 style='font-family:Symbol'>j</span></p> 00101 </td> 00102 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00103 <p align=center style='text-align:center'>0</p> 00104 </td> 00105 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00106 <p align=center style='text-align:center'>y</p> 00107 </td> 00108 </tr> 00109 <tr style='height:20.6pt'> 00110 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00111 <p align=center style='text-align:center'>0</p> 00112 </td> 00113 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00114 <p align=center style='text-align:center'>0</p> 00115 </td> 00116 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00117 <p align=center style='text-align:center'>1</p> 00118 </td> 00119 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00120 <p align=center style='text-align:center'>0</p> 00121 </td> 00122 </tr> 00123 <tr style='height:20.6pt'> 00124 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00125 <p align=center style='text-align:center'>0</p> 00126 </td> 00127 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00128 <p align=center style='text-align:center'>0</p> 00129 </td> 00130 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00131 <p align=center style='text-align:center'>0</p> 00132 </td> 00133 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00134 <p align=center style='text-align:center'>1</p> 00135 </td> 00136 </tr> 00137 </table> 00138 </div> 00139 <p align=center style='text-align:center'></p> 00140 </td> 00141 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00142 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00143 padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00144 <p align=center style='text-align:center'><img src="CPose2D.gif"></p> 00145 </td> 00146 </tr> 00147 </table> 00148 00149 </div> 00150 00151 * 00152 * \sa CPoseOrPoint,CPoint2D 00153 */ 00154 class MRPTDLLIMPEXP CPose2D : public CPose 00155 { 00156 // This must be added to any CSerializable derived class: 00157 DEFINE_SERIALIZABLE( CPose2D ) 00158 00159 protected: 00160 double m_phi; //!< The orientation of the pose, in radians. 00161 00162 public: 00163 /** Constructor from an initial value of the pose. 00164 */ 00165 CPose2D(const double& x=0,const double&y=0,const double& phi=0); 00166 00167 /** Constructor from a CPoint2D object. 00168 */ 00169 CPose2D(const CPoint2D &); 00170 00171 /** Aproximation!! Avoid its use, since information is lost. 00172 */ 00173 explicit CPose2D(const CPose3D &); 00174 00175 /** Constructor from lightweight object. 00176 */ 00177 CPose2D(const mrpt::math::TPose2D &); 00178 00179 /** Constructor from CPoint3D with information loss. 00180 */ 00181 explicit CPose2D(const CPoint3D &); 00182 00183 /** Get the phi angle of the 2D pose (in radians) */ 00184 double phi() const { return m_phi; } 00185 00186 /** Set the phi angle of the 2D pose (in radians) */ 00187 void phi(double angle) { m_phi=angle; } 00188 00189 void phi_incr(const double Aphi) { m_phi+=Aphi; } //!< Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed) 00190 00191 /** Returns a 1x3 vector with [x y phi] */ 00192 void getAsVector(vector_double &v) const; 00193 00194 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). 00195 * \sa getInverseHomogeneousMatrix 00196 */ 00197 void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const; 00198 00199 /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator. 00200 */ 00201 CPose2D operator + (const CPose2D& D) const ; 00202 00203 /** Makes \f$ this = A \oplus B \f$ 00204 * \note A or B can be "this" without problems. 00205 */ 00206 void composeFrom(const CPose2D &A, const CPose2D &B); 00207 00208 /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator. 00209 */ 00210 CPose3D operator + (const CPose3D& D) const ; 00211 00212 /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator. 00213 */ 00214 CPoint2D operator + (const CPoint2D& u) const ; 00215 00216 /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator. 00217 */ 00218 CPoint3D operator + (const CPoint3D& u) const ; 00219 00220 /** The operator \f$ D = this \ominus b \f$ is the pose inverse compounding operator, 00221 * the resulting pose D fulfils: \f$ this = b \oplus D \f$, that is: \f$ b = a \oplus (b \ominus a) \f$ 00222 */ 00223 CPose2D operator - (const CPose2D& b) const ; 00224 00225 /** Scalar sum of components: This is diferent from poses 00226 * composition, which is implemented as "+" operators in "CPose" derived classes. 00227 */ 00228 void AddComponents(CPose2D &p); 00229 00230 /** Scalar multiplication. 00231 */ 00232 void operator *=(const double &s); 00233 00234 /** Make \f$ this = this \oplus b \f$ */ 00235 CPose2D& operator += (const CPose2D& b) 00236 { 00237 composeFrom(*this,b); 00238 return *this; 00239 } 00240 00241 /** Forces "phi" to be in the range [-pi,pi]; 00242 */ 00243 void normalizePhi(); 00244 00245 }; // End of class def. 00246 00247 00248 std::ostream MRPTDLLIMPEXP & operator << (std::ostream& o, const CPose2D& p); 00249 00250 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y phi) */ 00251 CPose2D MRPTDLLIMPEXP operator -(const CPose2D &p); 00252 00253 bool MRPTDLLIMPEXP operator==(const CPose2D &p1,const CPose2D &p2); 00254 bool MRPTDLLIMPEXP operator!=(const CPose2D &p1,const CPose2D &p2); 00255 00256 } // End of namespace 00257 } // End of namespace 00258 00259 #endif
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |