Main MRPT website > C++ reference
MRPT logo
CPoseOrPoint.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPOSEORPOINT_H
29 #define CPOSEORPOINT_H
30 
31 #include <mrpt/math/CMatrixFixedNumeric.h>
32 #include <mrpt/math/lightweight_geom_data.h>
33 #include <mrpt/math/ops_matrices.h> // Added here so many classes have access to these templates
34 #include <mrpt/math/utils.h> // For homogeneousMatrixInverse
35 
36 #include <mrpt/poses/CPoseOrPoint_detail.h>
37 
38 namespace mrpt
39 {
40  /** \defgroup poses_grp 2D/3D points and poses
41  * \ingroup mrpt_base_grp */
42 
43  /** \defgroup poses_pdf_grp 2D/3D point and pose PDFs
44  * \ingroup mrpt_base_grp */
45 
46  /** Classes for 2D/3D geometry representation, both of single values and probability density distributions (PDFs) in many forms.
47  * \ingroup poses_grp poses_pdf_grp
48  */
49  namespace poses
50  {
51  using namespace mrpt::utils; // For square
52  using namespace mrpt::math; // For ligh. geom data
53 
54  // For use in some constructors (eg. CPose3D)
55  enum TConstructorFlags_Poses
56  {
57  UNINITIALIZED_POSE = 0
58  };
59 
60  /** The base template class for 2D & 3D points and poses.
61  * This class use the Curiously Recurring Template Pattern (CRTP) to define
62  * a set of common methods to all the children classes without the cost
63  * of virtual methods. Since most important methods are inline, they will be expanded
64  * at compile time and optimized for every specific derived case.
65  *
66  * For more information and examples, refer
67  * to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
68  *
69  *
70  * <center><h2>Introduction to 2D and 3D representation classes</h2></center>
71  * <hr>
72  * <p>
73  * There are two class of spatial representation classes:
74  * - Point: A point in the common mathematical sense, with no directional information.
75  * - 2D: A 2D point is represented just by its coordinates (x,y).
76  * - 3D: A 3D point is represented by its coordinates (x,y,z).
77  * - Pose: It is a point, plus a direction.
78  * - 2D: A 2D pose is a 2D point plus a single angle, the yaw or &#966; angle: the angle from the positive X angle.
79  * - 3D: A 3D point is a 3D point plus three orientation angles (More details above).
80  * </p>
81  * In the case for a 3D orientation many representation angles can be used (Euler angles,yaw/pitch/roll,...)
82  * but all of them can be handled by a 4x4 matrix called "Homogeneous Matrix". This matrix includes both, the
83  * translation and the orientation for a point or a pose, and it can be obtained using
84  * the method getHomogeneousMatrix() which is defined for any pose or point. Note that when the YPR angles are
85  * used to define a 3D orientation, these three values can not be extracted from the matrix again.<br><br>
86  *
87  * <b>Homogeneous matrices:</b> These are 4x4 matrices which can represent any translation or rotation in 2D & 3D.
88  * See the tutorial online for more details. *
89  *
90  * <b>Operators:</b> There are operators defined for the pose compounding \f$ \oplus \f$ and inverse pose
91  * compounding \f$ \ominus \f$ of poses and points. For example, let "a" and "b" be 2D or 3D poses. Then "a+b"
92  * returns the resulting pose of "moving b" from "a"; and "b-a" returns the pose of "b" as it is seen
93  * "from a". They can be mixed points and poses, being 2D or 3D, in these operators, with the following
94  * results: <br>
95  *
96  * <div align="center" >
97  * <pre>
98  * Does "a+b" return a Pose or a Point?
99  * +---------------------------------+
100  * | a \ b | Pose | Point |
101  * +----------+-----------+----------+
102  * | Pose | Pose | Point |
103  * | Point | Pose | Point |
104  * +---------------------------------+
105  *
106  * Does "a-b" return a Pose or a Point?
107  * +---------------------------------+
108  * | a \ b | Pose | Point |
109  * +----------+-----------+----------+
110  * | Pose | Pose | Pose |
111  * | Point | Point | Point |
112  * +---------------------------------+
113  *
114  * Does "a+b" and "a-b" return a 2D or 3D object?
115  * +-------------------------+
116  * | a \ b | 2D | 3D |
117  * +----------+--------------+
118  * | 2D | 2D | 3D |
119  * | 3D | 3D | 3D |
120  * +-------------------------+
121  *
122  * </pre>
123  * </div>
124  *
125  * \sa CPose,CPoint
126  * \ingroup poses_grp
127  */
128  template <class DERIVEDCLASS>
129  class CPoseOrPoint : public mrpt::poses::detail::pose_point_impl<DERIVEDCLASS, mrpt::poses::detail::T3DTypeHelper<DERIVEDCLASS>::is_3D_val>
130  {
131  public:
132  /** Common members of all points & poses classes.
133  @{ */
134  // Note: the access to "z" is implemented (only for 3D data types), in detail::pose_point_impl<>
135  inline double x() const /*!< Get X coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[0]; }
136  inline double y() const /*!< Get Y coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[1]; }
137 
138  inline double &x() /*!< Get ref to X coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[0]; }
139  inline double &y() /*!< Get ref to Y coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[1]; }
140 
141  inline void x(const double v) /*!< Set X coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]=v; }
142  inline void y(const double v) /*!< Set Y coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]=v; }
143 
144  inline void x_incr(const double v) /*!< X+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]+=v; }
145  inline void y_incr(const double v) /*!< Y+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]+=v; }
146 
147 
148  /** Return true for poses or points with a Z component, false otherwise. */
149  static inline bool is3DPoseOrPoint() { return DERIVEDCLASS::is_3D_val!=0; }
150 
151  /** Returns the squared euclidean distance to another pose/point: */
152  template <class OTHERCLASS> inline double sqrDistanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
153  {
154  if (b.is3DPoseOrPoint())
155  {
156  if (is3DPoseOrPoint())
157  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]);
158  else return square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
159  }
160  else
161  {
162  if (is3DPoseOrPoint())
163  return square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
164  else return square(x()-b.x()) + square(y()-b.y());
165  }
166  }
167 
168  /** Returns the Euclidean distance to another pose/point: */
169  template <class OTHERCLASS>
170  inline double distanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
171  {
172  return std::sqrt( sqrDistanceTo(b));
173  }
174 
175  /** Returns the squared 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
176  inline double distance2DToSquare( double ax, double ay ) const { return square(ax-x())+square(ay-y()); }
177 
178  /** Returns the squared 3D distance from this pose/point to a 3D point */
179  inline double distance3DToSquare( double ax, double ay, double az ) const {
180  return square(ax-x())+square(ay-y())+square(az-(is3DPoseOrPoint() ? static_cast<const DERIVEDCLASS*>(this)->m_coords[2] : 0) );
181  }
182 
183  /** Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
184  inline double distance2DTo( double ax, double ay ) const { return std::sqrt(distance2DToSquare(ax,ay)); }
185 
186  /** Returns the 3D distance from this pose/point to a 3D point */
187  inline double distance3DTo( double ax, double ay, double az ) const { return std::sqrt(distance3DToSquare(ax,ay,az)); }
188 
189  /** Returns the euclidean distance to a 3D point: */
190  inline double distanceTo(const mrpt::math::TPoint3D &b) const { return distance3DTo(b.x,b.y,b.z); }
191 
192  /** Returns the euclidean norm of vector: \f$ ||\mathbf{x}|| = \sqrt{x^2+y^2+z^2} \f$ */
193  inline double norm() const
194  {
195  return std::sqrt( square(x())+square(y())+ (!is3DPoseOrPoint() ? 0 : square(static_cast<const DERIVEDCLASS*>(this)->m_coords[2]) ) );
196  }
197 
198  /** Return the pose or point as a 1xN vector with all the components (see derived classes for each implementation) */
199  inline vector_double getAsVectorVal() const
200  {
201  vector_double v;
202  static_cast<const DERIVEDCLASS*>(this)->getAsVector(v);
203  return v;
204  }
205 
206  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
207  * \sa getInverseHomogeneousMatrix
208  */
209  inline CMatrixDouble44 getHomogeneousMatrixVal() const
210  {
211  CMatrixDouble44 m(UNINITIALIZED_MATRIX);
212  static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(m);
213  return m;
214  }
215 
216  /** Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose.
217  * \sa getHomogeneousMatrix
218  */
219  inline void getInverseHomogeneousMatrix( math::CMatrixDouble44 &out_HM ) const
220  { // Get current HM & inverse in-place:
221  static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(out_HM);
222  mrpt::math::homogeneousMatrixInverse(out_HM);
223  }
224 
225  //! \overload
226  inline mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
227  {
228  mrpt::math::CMatrixDouble44 M(UNINITIALIZED_MATRIX);
229  getInverseHomogeneousMatrix(M);
230  return M;
231  }
232 
233  /** @} */
234  }; // End of class def.
235 
236 
237  } // End of namespace
238 } // End of namespace
239 
240 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013