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