Main MRPT website > C++ reference
MRPT logo

CPoseOrPoint.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  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/math/CMatrixFixedNumeric.h>
00032 #include <mrpt/math/lightweight_geom_data.h>
00033 #include <mrpt/math/ops_matrices.h>  // Added here so many classes have access to these templates
00034 #include <mrpt/math/utils.h>             // For homogeneousMatrixInverse
00035 
00036 #include <mrpt/poses/CPoseOrPoint_detail.h>
00037 
00038 namespace mrpt
00039 {
00040         /** Classes for 2D/3D geometry representation, both of single values and probability density distributions (PDFs) in many forms.
00041           */
00042         namespace poses
00043         {
00044                 using namespace mrpt::utils;  // For square
00045                 using namespace mrpt::math;  // For ligh. geom data
00046 
00047                 // For use in some constructors (eg. CPose3D)
00048                 enum TConstructorFlags_Poses
00049                 {
00050                         UNINITIALIZED_POSE = 0
00051                 };
00052 
00053                 /** The base template class for 2D & 3D points and poses.
00054                  *   This class use the Curiously Recurring Template Pattern (CRTP) to define
00055                  *    a set of common methods to all the children classes without the cost
00056                  *    of virtual methods. Since most important methods are inline, they will be expanded
00057                  *    at compile time and optimized for every specific derived case.
00058                  *
00059                  *  For more information and examples, refer
00060                  *    to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
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 &#966; 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>Homogeneous matrices:</b> These are 4x4 matrices which can represent any translation or rotation in 2D & 3D.
00081                  *     See the tutorial online for more details.                         *
00082                  *
00083                  *  <b>Operators:</b> There are operators defined for the pose compounding \f$ \oplus \f$ and inverse pose
00084                  *   compounding \f$ \ominus \f$ 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                  *
00089                  * <div align="center" >
00090                  *  <pre>
00091                  *  Does "a+b" return a Pose or a Point?
00092                  * +---------------------------------+
00093                  * |  a \ b   |  Pose     |  Point   |
00094                  * +----------+-----------+----------+
00095                  * | Pose     |  Pose     |  Point   |
00096                  * | Point    |  Pose     |  Point   |
00097                  * +---------------------------------+
00098                  *
00099                  *  Does "a-b" return a Pose or a Point?
00100                  * +---------------------------------+
00101                  * |  a \ b   |  Pose     |  Point   |
00102                  * +----------+-----------+----------+
00103                  * | Pose     |  Pose     |  Pose    |
00104                  * | Point    |  Point    |  Point   |
00105                  * +---------------------------------+
00106                  *
00107                  *  Does "a+b" and "a-b" return a 2D or 3D object?
00108                  * +-------------------------+
00109                  * |  a \ b   |  2D   |  3D  |
00110                  * +----------+--------------+
00111                  * |  2D      |  2D   |  3D  |
00112                  * |  3D      |  3D   |  3D  |
00113                  * +-------------------------+
00114                  *
00115                  *  </pre>
00116                  * </div>
00117                  *
00118                  * \sa CPose,CPoint
00119                  */
00120                 template <class DERIVEDCLASS>
00121                 class CPoseOrPoint : public mrpt::poses::detail::pose_point_impl<DERIVEDCLASS, mrpt::poses::detail::T3DTypeHelper<DERIVEDCLASS>::is_3D_val>
00122                 {
00123                 public:
00124                         /** Common members of all points & poses classes.
00125                             @{ */
00126                         // Note: the access to "z" is implemented (only for 3D data types), in detail::pose_point_impl<>
00127                         inline double x() const /*!< Get X coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[0]; }
00128                         inline double y() const /*!< Get Y coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[1]; }
00129 
00130                         inline double &x() /*!< Get ref to X coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[0]; }
00131                         inline double &y() /*!< Get ref to Y coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[1]; }
00132 
00133                         inline void x(const double v) /*!< Set X coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]=v; }
00134                         inline void y(const double v) /*!< Set Y coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]=v; }
00135 
00136                         inline void x_incr(const double v) /*!< X+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]+=v; }
00137                         inline void y_incr(const double v) /*!< Y+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]+=v; }
00138 
00139 
00140                         /** Return true for poses or points with a Z component, false otherwise. */
00141                         static inline bool is3DPoseOrPoint() { return DERIVEDCLASS::is_3D_val!=0; }
00142 
00143                         /** Returns the squared euclidean distance to another pose/point: */
00144                         template <class OTHERCLASS>     inline double sqrDistanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
00145                         {
00146                                 if (b.is3DPoseOrPoint())
00147                                 {
00148                                         if (is3DPoseOrPoint())
00149                                                   return  square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const DERIVEDCLASS*>(this)->m_coords[2]-static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
00150                                         else  return  square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
00151                                 }
00152                                 else
00153                                 {
00154                                         if (is3DPoseOrPoint())
00155                                                   return  square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
00156                                         else  return  square(x()-b.x()) + square(y()-b.y());
00157                                 }
00158                         }
00159 
00160                         /** Returns the Euclidean distance to another pose/point: */
00161                         template <class OTHERCLASS>
00162                         inline double distanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
00163                         {
00164                                 return std::sqrt( sqrDistanceTo(b));
00165                         }
00166 
00167                         /** Returns the squared 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
00168                         inline double distance2DToSquare( double ax, double ay ) const { return  square(ax-x())+square(ay-y()); }
00169 
00170                         /** Returns the squared 3D distance from this pose/point to a 3D point */
00171                         inline double distance3DToSquare( double ax, double ay, double az ) const {
00172                                 return square(ax-x())+square(ay-y())+square(az-(is3DPoseOrPoint() ? static_cast<const DERIVEDCLASS*>(this)->m_coords[2] : 0) );
00173                         }
00174 
00175                         /** Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
00176                         inline double distance2DTo( double ax, double ay ) const { return std::sqrt(distance2DToSquare(ax,ay)); }
00177 
00178                         /** Returns the 3D distance from this pose/point to a 3D point */
00179                         inline double distance3DTo( double ax, double ay, double az ) const { return std::sqrt(distance3DToSquare(ax,ay,az));  }
00180 
00181                         /** Returns the euclidean distance to a 3D point: */
00182                         inline double distanceTo(const mrpt::math::TPoint3D &b) const { return distance3DTo(b.x,b.y,b.z); }
00183 
00184                         /** Returns the euclidean norm of vector: \f$ ||\mathbf{x}|| = \sqrt{x^2+y^2+z^2} \f$ */
00185                         inline double  norm() const
00186                         {
00187                                 return std::sqrt( square(x())+square(y())+ (!is3DPoseOrPoint() ? 0 : square(static_cast<const DERIVEDCLASS*>(this)->m_coords[2]) ) );
00188                         }
00189 
00190                         /** Return the pose or point as a 1xN vector with all the components (see derived classes for each implementation) */
00191                         inline vector_double getAsVectorVal() const
00192                         {
00193                                 vector_double v;
00194                                 static_cast<const DERIVEDCLASS*>(this)->getAsVector(v);
00195                                 return v;
00196                         }
00197 
00198                         /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
00199                         * \sa getInverseHomogeneousMatrix
00200                         */
00201                         inline CMatrixDouble44 getHomogeneousMatrixVal() const
00202                         {
00203                                 CMatrixDouble44 m(UNINITIALIZED_MATRIX);
00204                                 static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(m);
00205                                 return m;
00206                         }
00207 
00208                         /** Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose.
00209                         * \sa getHomogeneousMatrix
00210                         */
00211                         inline void getInverseHomogeneousMatrix( math::CMatrixDouble44 &out_HM ) const
00212                         {       // Get current HM & inverse in-place:
00213                                 static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(out_HM);
00214                                 mrpt::math::homogeneousMatrixInverse(out_HM);
00215                         }
00216 
00217                         //! \overload
00218                         inline mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
00219                         {
00220                                 mrpt::math::CMatrixDouble44 M(UNINITIALIZED_MATRIX);
00221                                 getInverseHomogeneousMatrix(M);
00222                                 return M;
00223                         }
00224 
00225                         /** @} */
00226                 }; // End of class def.
00227 
00228 
00229         } // End of namespace
00230 } // End of namespace
00231 
00232 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011