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