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 CPOSEORPOINT_H 00029 #define CPOSEORPOINT_H 00030 00031 #include <mrpt/utils/CSerializable.h> 00032 #include <mrpt/math/CMatrixFixedNumeric.h> 00033 #include <mrpt/math/lightweight_geom_data.h> 00034 00035 namespace mrpt 00036 { 00037 /** Classes for 2D/3D geometry representation, both of single values and probability density distributions (PDFs) in many forms. 00038 */ 00039 namespace poses 00040 { 00041 using namespace mrpt::utils; // For square 00042 using namespace mrpt::math; // For ligh. geom data 00043 00044 class CPoint2D; 00045 class CPoint3D; 00046 class CPose2D; 00047 class CPose3D; 00048 00049 // This must be added to any CSerializable derived class: 00050 DEFINE_SERIALIZABLE_PRE( CPoseOrPoint ) 00051 00052 /** The base class for 2D points, 3D points, 2D poses and 3D poses. 00053 * The common part defined in this base is the N-dimensional (N=2,3) 00054 * <b>position</b> vector. The existence of orientation angles is 00055 * left to derived classes. 00056 * In this class euclidean distance methods and operators are implemented. 00057 * 00058 * For more information and examples, refer 00059 * to the <a href="http://babel.isa.uma.es/mrpt/index.php/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki. 00060 * 00061 00062 <center><h2>Introduction to 2D and 3D representation classes</h2></center> 00063 <hr> 00064 <p> 00065 There are two class of spatial representation classes: 00066 - Point: A point in the common mathematical sense, with no directional information. 00067 - 2D: A 2D point is represented just by its coordinates (x,y). 00068 - 3D: A 3D point is represented by its coordinates (x,y,z). 00069 - Pose: It is a point, plus a direction. 00070 - 2D: A 2D pose is a 2D point plus a single angle, the yaw or φ angle: the angle from the positive X angle. 00071 - 3D: A 3D point is a 3D point plus three orientation angles (More details above). 00072 </p> 00073 In the case for a 3D orientation many representation angles can be used (Euler angles,yaw/pitch/roll,...) 00074 but all of them can be handled by a 4x4 matrix called "Homogeneous Matrix". This matrix includes both, the 00075 translation and the orientation for a point or a pose, and it can be obtained using 00076 the method getHomogeneousMatrix() which is defined for any pose or point. Note that when the YPR angles are 00077 used to define a 3D orientation, these three values can not be extracted from the matrix again.<br><br> 00078 00079 <b>Operators:</b> There are operators defined for the pose compounding (+) and inverse pose 00080 compounding (-) of poses and points. For example, let "a" and "b" be 2D or 3D poses. Then "a+b" 00081 returns the resulting pose of "moving b" from "a"; and "b-a" returns the pose of "b" as it is seen 00082 "from a". They can be mixed points and poses, being 2D or 3D, in these operators, with the following 00083 results: <br> 00084 <center> 00085 Does "a+b"returns a Pose or a Point? 00086 <table> 00087 <tr> 00088 <td><b>a \ b</b></td> 00089 <td><b>Pose</b></td> 00090 <td><b>Point</b></td> 00091 </tr> 00092 <tr> 00093 <td><b>Pose</b></td> 00094 <td>Pose</td> 00095 <td>Point</td> 00096 </tr> 00097 <tr> 00098 <td><b>Point</b></td> 00099 <td>Pose</td> 00100 <td>Point</td> 00101 </tr> 00102 </table> 00103 </center> 00104 <br> 00105 <center> 00106 Does "a-b"returns a Pose or a Point? 00107 <table> 00108 <tr> 00109 <td><b>a \ b</b></td> 00110 <td><b>Pose</b></td> 00111 <td><b>Point</b></td> 00112 </tr> 00113 <tr> 00114 <td><b>Pose</b></td> 00115 <td>Pose</td> 00116 <td>Pose</td> 00117 </tr> 00118 <tr> 00119 <td><b>Point</b></td> 00120 <td>Point</td> 00121 <td>Point</td> 00122 </tr> 00123 </table> 00124 </center> 00125 <br> 00126 <center> 00127 Does "a+b"(and "a-b") returns a 2D or a 3D object? 00128 <table> 00129 <tr> 00130 <td><b>a \ b</b></td> 00131 <td><b>2D</b></td> 00132 <td><b>3D</b></td> 00133 </tr> 00134 <tr> 00135 <td><b>2D</b></td> 00136 <td>2D</td> 00137 <td>3D</td> 00138 </tr> 00139 <tr> 00140 <td><b>3D</b></td> 00141 <td>3D</td> 00142 <td>3D</td> 00143 </tr> 00144 </table> 00145 </center> 00146 <br><br> 00147 <b>Homogeneous matrices:</b> The matrices computation follows the equations that can be found in 00148 any introductory text to spatial orientations, which are exposed next:<br> 00149 00150 <div align=center> 00151 00152 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00153 style='border-collapse:collapse;border:none'> 00154 <tr> 00155 <td width=576 colspan=2 style='width:432.2pt;border:solid windowtext 1.0pt; 00156 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt'> 00157 <p align=center style='text-align:center'>poses::CPoint2D</p> 00158 </td> 00159 </tr> 00160 <tr> 00161 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00162 none;padding:0cm 5.4pt 0cm 5.4pt'> 00163 <p align=center style='text-align:center'>Homogeneous 00164 transfomation matrix</p> 00165 </td> 00166 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00167 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00168 padding:0cm 5.4pt 0cm 5.4pt'> 00169 <p align=center style='text-align:center'>Spatial 00170 representation</p> 00171 </td> 00172 </tr> 00173 <tr style='height:108.3pt'> 00174 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00175 none;padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00176 <div align=center> 00177 <table Table border=0 cellspacing=0 cellpadding=0 width="46%" 00178 style='width:46.84%;border-collapse:collapse'> 00179 <tr style='height:16.5pt'> 00180 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00181 <p align=center style='text-align:center'>1</p> 00182 </td> 00183 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00184 <p align=center style='text-align:center'>0</p> 00185 </td> 00186 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00187 <p align=center style='text-align:center'>0</p> 00188 </td> 00189 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00190 <p align=center style='text-align:center'>x</p> 00191 </td> 00192 </tr> 00193 <tr style='height:16.5pt'> 00194 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00195 <p align=center style='text-align:center'>0</p> 00196 </td> 00197 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00198 <p align=center style='text-align:center'>1</p> 00199 </td> 00200 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00201 <p align=center style='text-align:center'>0</p> 00202 </td> 00203 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00204 <p align=center style='text-align:center'>y</p> 00205 </td> 00206 </tr> 00207 <tr style='height:16.5pt'> 00208 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00209 <p align=center style='text-align:center'>0</p> 00210 </td> 00211 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00212 <p align=center style='text-align:center'>0</p> 00213 </td> 00214 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00215 <p align=center style='text-align:center'>1</p> 00216 </td> 00217 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00218 <p align=center style='text-align:center'>0</p> 00219 </td> 00220 </tr> 00221 <tr style='height:16.5pt'> 00222 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00223 <p align=center style='text-align:center'>0</p> 00224 </td> 00225 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00226 <p align=center style='text-align:center'>0</p> 00227 </td> 00228 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00229 <p align=center style='text-align:center'>0</p> 00230 </td> 00231 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00232 <p align=center style='text-align:center'>1</p> 00233 </td> 00234 </tr> 00235 </table> 00236 </div> 00237 <p align=center style='text-align:center'></p> 00238 </td> 00239 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00240 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00241 padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00242 <p align=center style='text-align:center'><img src="CPoint2D.gif"></p> 00243 </td> 00244 </tr> 00245 </table> 00246 00247 </div> 00248 00249 <p></p> 00250 00251 <div align=center> 00252 00253 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00254 style='border-collapse:collapse;border:none'> 00255 <tr> 00256 <td width=576 colspan=2 style='width:432.2pt;border:solid windowtext 1.0pt; 00257 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt'> 00258 <p align=center style='text-align:center'>poses::CPoint3D</p> 00259 </td> 00260 </tr> 00261 <tr> 00262 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00263 none;padding:0cm 5.4pt 0cm 5.4pt'> 00264 <p align=center style='text-align:center'>Homogeneous 00265 transfomation matrix</p> 00266 </td> 00267 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00268 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00269 padding:0cm 5.4pt 0cm 5.4pt'> 00270 <p align=center style='text-align:center'>Spatial 00271 representation</p> 00272 </td> 00273 </tr> 00274 <tr style='height:108.3pt'> 00275 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00276 none;padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00277 <div align=center> 00278 <table Table border=0 cellspacing=0 cellpadding=0 width="46%" 00279 style='width:46.84%;border-collapse:collapse'> 00280 <tr style='height:16.5pt'> 00281 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00282 <p align=center style='text-align:center'>1</p> 00283 </td> 00284 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00285 <p align=center style='text-align:center'>0</p> 00286 </td> 00287 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00288 <p align=center style='text-align:center'>0</p> 00289 </td> 00290 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00291 <p align=center style='text-align:center'>x</p> 00292 </td> 00293 </tr> 00294 <tr style='height:16.5pt'> 00295 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00296 <p align=center style='text-align:center'>0</p> 00297 </td> 00298 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00299 <p align=center style='text-align:center'>1</p> 00300 </td> 00301 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00302 <p align=center style='text-align:center'>0</p> 00303 </td> 00304 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00305 <p align=center style='text-align:center'>y</p> 00306 </td> 00307 </tr> 00308 <tr style='height:16.5pt'> 00309 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00310 <p align=center style='text-align:center'>0</p> 00311 </td> 00312 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00313 <p align=center style='text-align:center'>0</p> 00314 </td> 00315 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00316 <p align=center style='text-align:center'>1</p> 00317 </td> 00318 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00319 <p align=center style='text-align:center'>z</p> 00320 </td> 00321 </tr> 00322 <tr style='height:16.5pt'> 00323 <td width=32 style='width:24.0pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00324 <p align=center style='text-align:center'>0</p> 00325 </td> 00326 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00327 <p align=center style='text-align:center'>0</p> 00328 </td> 00329 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00330 <p align=center style='text-align:center'>0</p> 00331 </td> 00332 <td width=32 style='width:24.05pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.5pt'> 00333 <p align=center style='text-align:center'>1</p> 00334 </td> 00335 </tr> 00336 </table> 00337 </div> 00338 <p align=center style='text-align:center'></p> 00339 </td> 00340 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00341 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00342 padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00343 <p align=center style='text-align:center'><img src="CPoint3D.gif"></p> 00344 </td> 00345 </tr> 00346 </table> 00347 00348 </div> 00349 00350 <p></p> 00351 00352 <div align=center> 00353 00354 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00355 style='border-collapse:collapse;border:none'> 00356 <tr> 00357 <td width=576 colspan=2 style='width:432.2pt;border:solid windowtext 1.0pt; 00358 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt'> 00359 <p align=center style='text-align:center'>poses::CPose2D</p> 00360 </td> 00361 </tr> 00362 <tr> 00363 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00364 none;padding:0cm 5.4pt 0cm 5.4pt'> 00365 <p align=center style='text-align:center'>Homogeneous 00366 transfomation matrix</p> 00367 </td> 00368 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00369 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00370 padding:0cm 5.4pt 0cm 5.4pt'> 00371 <p align=center style='text-align:center'>Spatial 00372 representation</p> 00373 </td> 00374 </tr> 00375 <tr style='height:108.3pt'> 00376 <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top: 00377 none;padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00378 <div align=center> 00379 <table Table border=0 cellspacing=0 cellpadding=0 width="67%" 00380 style='width:67.92%;border-collapse:collapse'> 00381 <tr style='height:20.6pt'> 00382 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00383 <p align=center style='text-align:center'>cos<span 00384 style='font-family:Symbol'>j</span></p> 00385 </td> 00386 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00387 <p align=center style='text-align:center'>-sin<span 00388 style='font-family:Symbol'>j</span></p> 00389 </td> 00390 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00391 <p align=center style='text-align:center'>0</p> 00392 </td> 00393 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00394 <p align=center style='text-align:center'>x</p> 00395 </td> 00396 </tr> 00397 <tr style='height:20.6pt'> 00398 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00399 <p align=center style='text-align:center'>sin<span 00400 style='font-family:Symbol'>j</span></p> 00401 </td> 00402 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00403 <p align=center style='text-align:center'>cos<span 00404 style='font-family:Symbol'>j</span></p> 00405 </td> 00406 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00407 <p align=center style='text-align:center'>0</p> 00408 </td> 00409 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00410 <p align=center style='text-align:center'>y</p> 00411 </td> 00412 </tr> 00413 <tr style='height:20.6pt'> 00414 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00415 <p align=center style='text-align:center'>0</p> 00416 </td> 00417 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00418 <p align=center style='text-align:center'>0</p> 00419 </td> 00420 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00421 <p align=center style='text-align:center'>1</p> 00422 </td> 00423 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00424 <p align=center style='text-align:center'>0</p> 00425 </td> 00426 </tr> 00427 <tr style='height:20.6pt'> 00428 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00429 <p align=center style='text-align:center'>0</p> 00430 </td> 00431 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00432 <p align=center style='text-align:center'>0</p> 00433 </td> 00434 <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00435 <p align=center style='text-align:center'>0</p> 00436 </td> 00437 <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'> 00438 <p align=center style='text-align:center'>1</p> 00439 </td> 00440 </tr> 00441 </table> 00442 </div> 00443 <p align=center style='text-align:center'></p> 00444 </td> 00445 <td width=288 style='width:216.1pt;border-top:none;border-left:none; 00446 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00447 padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'> 00448 <p align=center style='text-align:center'><img src="CPose2D.gif"></p> 00449 </td> 00450 </tr> 00451 </table> 00452 00453 </div> 00454 00455 <p></p> 00456 00457 <div align=center> 00458 00459 <table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0 00460 style='border-collapse:collapse;border:none'> 00461 <tr style='height:15.8pt'> 00462 <td width=676 colspan=2 style='width:507.25pt;border:solid windowtext 1.0pt; 00463 background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00464 <p align=center style='text-align:center'>poses::CPose3D</p> 00465 </td> 00466 </tr> 00467 <tr style='height:15.8pt'> 00468 <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top: 00469 none;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00470 <p align=center style='text-align:center'>Homogeneous 00471 transfomation matrix</p> 00472 </td> 00473 <td width=326 style='width:244.6pt;border-top:none;border-left:none; 00474 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00475 padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'> 00476 <p align=center style='text-align:center'>Spatial 00477 representation</p> 00478 </td> 00479 </tr> 00480 <tr style='height:202.65pt'> 00481 <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top: 00482 none;padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'> 00483 <div align=center> 00484 <table Table border=0 cellspacing=0 cellpadding=0 width=334 00485 style='width:250.65pt;border-collapse:collapse'> 00486 <tr style='height:16.65pt'> 00487 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00488 <p align=center style='text-align:center'>cycp</p> 00489 </td> 00490 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00491 <p align=center style='text-align:center'>cyspsr-sycr</p> 00492 </td> 00493 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00494 <p align=center style='text-align:center'>cyspcr+sysr</p> 00495 </td> 00496 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'> 00497 <p align=center style='text-align:center'>x</p> 00498 </td> 00499 </tr> 00500 <tr style='height:17.25pt'> 00501 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00502 <p align=center style='text-align:center'>sycp</p> 00503 </td> 00504 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00505 <p align=center style='text-align:center'>syspsr+cycr</p> 00506 </td> 00507 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00508 <p align=center style='text-align:center'>syspcr-cysr</p> 00509 </td> 00510 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'> 00511 <p align=center style='text-align:center'>y</p> 00512 </td> 00513 </tr> 00514 <tr style='height:19.65pt'> 00515 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00516 <p align=center style='text-align:center'>-sp</p> 00517 </td> 00518 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00519 <p align=center style='text-align:center'>cpsr</p> 00520 </td> 00521 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00522 <p align=center style='text-align:center'>cpcr</p> 00523 </td> 00524 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'> 00525 <p align=center style='text-align:center'>z</p> 00526 </td> 00527 </tr> 00528 <tr style='height:11.0pt'> 00529 <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00530 <p align=center style='text-align:center'>0</p> 00531 </td> 00532 <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00533 <p align=center style='text-align:center'>0</p> 00534 </td> 00535 <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00536 <p align=center style='text-align:center'>0</p> 00537 </td> 00538 <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'> 00539 <p align=center style='text-align:center'>1</p> 00540 </td> 00541 </tr> 00542 </table> 00543 </div> 00544 <p align=center style='text-align:center'><span lang=EN-GB>where:</span></p> 00545 <p align=center style='text-align:center'><span lang=EN-GB>cy 00546 = cos Yaw ; sy = sin Yaw</span></p> 00547 <p align=center style='text-align:center'><span lang=EN-GB>cp 00548 = cos Pitch ; sp = sin Pitch</span></p> 00549 <p align=center style='text-align:center'><span lang=EN-GB>cr 00550 = cos Roll ; sr = sin Roll</span></p> 00551 </td> 00552 <td width=326 style='width:244.6pt;border-top:none;border-left:none; 00553 border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt; 00554 padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'> 00555 <p align=center style='text-align:center'><span lang=EN-GB><img src="CPose3D.gif"></span></p> 00556 </td> 00557 </tr> 00558 </table> 00559 00560 </div> 00561 00562 * \sa CPose,CPoint 00563 */ 00564 class MRPTDLLIMPEXP CPoseOrPoint : public mrpt::utils::CSerializable 00565 { 00566 // This must be added to any CSerializable derived class: 00567 DEFINE_VIRTUAL_SERIALIZABLE( CPoseOrPoint ) 00568 00569 protected: 00570 bool m_is3D; //!< Will be false for 2D points or poses, and true for 3D points or poses. 00571 double m_x, m_y, m_z; //!< The x y and z coordinates of the point or pose. 00572 00573 public: 00574 CPoseOrPoint() : m_is3D(), m_x(),m_y(),m_z() 00575 { } 00576 00577 double x() const { return m_x; } //!< Get the X coordinate 00578 double y() const { return m_y; } //!< Get the Y coordinate 00579 double z() const { return m_z; } //!< Get the Z coordinate 00580 00581 virtual void x(const double x_) { m_x=x_; } //!< Set the X coordinate 00582 virtual void y(const double y_) { m_y=y_; } //!< Set the Y coordinate 00583 virtual void z(const double z_) { m_z=z_; } //!< Set the Z coordinate 00584 00585 virtual void x_incr(const double Ax) { m_x+=Ax; } //!< Increment the X coordinate 00586 virtual void y_incr(const double Ay) { m_y+=Ay; } //!< Increment the Y coordinate 00587 virtual void z_incr(const double Az) { m_z+=Az; } //!< Increment the Z coordinate 00588 00589 /** Return true for poses or points with a Z component, false otherwise. 00590 */ 00591 bool is3DPoseOrPoint() const { return m_is3D; } 00592 00593 /** Returns the euclidean distance to another pose/point: 00594 */ 00595 double distanceTo(const CPoseOrPoint &b) const 00596 { 00597 if (m_is3D || b.m_is3D) 00598 return sqrt(square(m_x-b.m_x) + square(m_y-b.m_y) + square(m_z-b.m_z)); 00599 else return sqrt(square(m_x-b.m_x) + square(m_y-b.m_y)); 00600 } 00601 00602 /** Returns the euclidean distance to another pose/point: 00603 */ 00604 double distanceTo(const mrpt::math::TPoint3D &b) const 00605 { 00606 if (m_is3D) 00607 return sqrt(square(m_x-b.x) + square(m_y-b.y) + square(m_z-b.z)); 00608 else return sqrt(square(m_x-b.x) + square(m_y-b.y)); 00609 } 00610 00611 /** Returns the squared euclidean distance to another pose/point: 00612 */ 00613 double sqrDistanceTo(const CPoseOrPoint &b) const 00614 { 00615 if (m_is3D || b.m_is3D) 00616 return square(m_x-b.m_x) + square(m_y-b.m_y) + square(m_z-b.m_z); 00617 else return square(m_x-b.m_x) + square(m_y-b.m_y); 00618 } 00619 00620 /** Returns the euclidean norm of vector: \f$ ||\mathbf{x}|| = \sqrt{x^2_1+y^2_i+...} \f$ 00621 */ 00622 double norm() const; 00623 00624 /** Scalar multiplication. 00625 */ 00626 virtual void operator *=(const double &s) = 0; 00627 00628 /** Return the pose or point as a 1xN vector with all the components (see derived classes for each implementation) */ 00629 virtual void getAsVector(vector_double &v) const = 0; 00630 00631 /** Return the pose or point as a 1xN vector with all the components (see derived classes for each implementation) */ 00632 vector_double getAsVectorVal() const 00633 { 00634 vector_double v; 00635 this->getAsVector(v); 00636 return v; 00637 } 00638 00639 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). 00640 * \sa getInverseHomogeneousMatrix 00641 */ 00642 CMatrixDouble44 getHomogeneousMatrixVal() const { 00643 CMatrixDouble44 m(false,false); 00644 getHomogeneousMatrix(m); 00645 return m; 00646 } 00647 00648 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation). 00649 * \sa getHomogeneousMatrixVal, getInverseHomogeneousMatrix 00650 */ 00651 virtual void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const =0; 00652 00653 /** Returns the corresponding inverse homogeneous 00654 * transformation matrix for the point(translation), 00655 * or pose (translation+orientation). 00656 * \sa getHomogeneousMatrix 00657 */ 00658 void getInverseHomogeneousMatrix( math::CMatrixDouble44 &out_HM ) const; 00659 00660 /** Returns the 2D distance from this pose/point to another given by its coordinates: 00661 * \sa distance2DToSquare,distance3DTo 00662 */ 00663 double distance2DTo( double ax, double ay ) const { return sqrt( square(ax-m_x)+square(ay-m_y) ); } 00664 00665 /** Returns the 3D distance from this pose/point to another given by its coordinates: 00666 * \sa distance3DToSquare,distance2DTo 00667 */ 00668 double distance3DTo( double ax, double ay, double az ) const { return sqrt( square(ax-m_x)+square(ay-m_y)+square(az-m_z)); } 00669 00670 /** Returns the square of the 2D distance from this pose/point to another given by its coordinates: 00671 * \sa distance2DTo,distance3DToSquare 00672 */ 00673 double distance2DToSquare( double ax, double ay ) const { return square(ax-m_x)+square(ay-m_y); } 00674 00675 /** Returns the square of the the 3D distance from this pose/point to another given by its coordinates: 00676 * \sa distance3DTo,distance2DToSquare 00677 */ 00678 double distance3DToSquare( double ax, double ay, double az ) const { return square(ax-m_x)+square(ay-m_y)+square(az-m_z); } 00679 00680 }; // End of class def. 00681 00682 00683 } // End of namespace 00684 } // End of namespace 00685 00686 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |