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 φ 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:exported at Tue Jan 25 21:56:31 UTC 2011 |