Main MRPT website > C++ reference
MRPT logo
CPoseOrPoint.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CPOSEORPOINT_H
10 #define CPOSEORPOINT_H
11 
14 #include <mrpt/math/ops_matrices.h> // Added here so many classes have access to these templates
15 #include <mrpt/math/utils.h> // For homogeneousMatrixInverse
16 
18 
19 namespace mrpt
20 {
21  /** \defgroup poses_grp 2D/3D points and poses
22  * \ingroup mrpt_base_grp */
23 
24  /** \defgroup poses_pdf_grp 2D/3D point and pose PDFs
25  * \ingroup mrpt_base_grp */
26 
27  /** Classes for 2D/3D geometry representation, both of single values and probability density distributions (PDFs) in many forms.
28  * \ingroup poses_grp poses_pdf_grp
29  */
30  namespace poses
31  {
32  using namespace mrpt::utils; // For square
33  using namespace mrpt::math; // For ligh. geom data
34 
35  // For use in some constructors (eg. CPose3D)
37  {
39  };
40 
41  /** The base template class for 2D & 3D points and poses.
42  * This class use the Curiously Recurring Template Pattern (CRTP) to define
43  * a set of common methods to all the children classes without the cost
44  * of virtual methods. Since most important methods are inline, they will be expanded
45  * at compile time and optimized for every specific derived case.
46  *
47  * For more information and examples, refer
48  * to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
49  *
50  *
51  * <center><h2>Introduction to 2D and 3D representation classes</h2></center>
52  * <hr>
53  * <p>
54  * There are two class of spatial representation classes:
55  * - Point: A point in the common mathematical sense, with no directional information.
56  * - 2D: A 2D point is represented just by its coordinates (x,y).
57  * - 3D: A 3D point is represented by its coordinates (x,y,z).
58  * - Pose: It is a point, plus a direction.
59  * - 2D: A 2D pose is a 2D point plus a single angle, the yaw or &#966; angle: the angle from the positive X angle.
60  * - 3D: A 3D point is a 3D point plus three orientation angles (More details above).
61  * </p>
62  * In the case for a 3D orientation many representation angles can be used (Euler angles,yaw/pitch/roll,...)
63  * but all of them can be handled by a 4x4 matrix called "Homogeneous Matrix". This matrix includes both, the
64  * translation and the orientation for a point or a pose, and it can be obtained using
65  * the method getHomogeneousMatrix() which is defined for any pose or point. Note that when the YPR angles are
66  * used to define a 3D orientation, these three values can not be extracted from the matrix again.<br><br>
67  *
68  * <b>Homogeneous matrices:</b> These are 4x4 matrices which can represent any translation or rotation in 2D & 3D.
69  * See the tutorial online for more details. *
70  *
71  * <b>Operators:</b> There are operators defined for the pose compounding \f$ \oplus \f$ and inverse pose
72  * compounding \f$ \ominus \f$ of poses and points. For example, let "a" and "b" be 2D or 3D poses. Then "a+b"
73  * returns the resulting pose of "moving b" from "a"; and "b-a" returns the pose of "b" as it is seen
74  * "from a". They can be mixed points and poses, being 2D or 3D, in these operators, with the following
75  * results: <br>
76  *
77  * <div align="center" >
78  * <pre>
79  * Does "a+b" return a Pose or a Point?
80  * +---------------------------------+
81  * | a \ b | Pose | Point |
82  * +----------+-----------+----------+
83  * | Pose | Pose | Point |
84  * | Point | Pose | Point |
85  * +---------------------------------+
86  *
87  * Does "a-b" return a Pose or a Point?
88  * +---------------------------------+
89  * | a \ b | Pose | Point |
90  * +----------+-----------+----------+
91  * | Pose | Pose | Pose |
92  * | Point | Point | Point |
93  * +---------------------------------+
94  *
95  * Does "a+b" and "a-b" return a 2D or 3D object?
96  * +-------------------------+
97  * | a \ b | 2D | 3D |
98  * +----------+--------------+
99  * | 2D | 2D | 3D |
100  * | 3D | 3D | 3D |
101  * +-------------------------+
102  *
103  * </pre>
104  * </div>
105  *
106  * \sa CPose,CPoint
107  * \ingroup poses_grp
108  */
109  template <class DERIVEDCLASS>
110  class CPoseOrPoint : public mrpt::poses::detail::pose_point_impl<DERIVEDCLASS, mrpt::poses::detail::T3DTypeHelper<DERIVEDCLASS>::is_3D_val>
111  {
112  public:
113  /** Common members of all points & poses classes.
114  @{ */
115  // Note: the access to "z" is implemented (only for 3D data types), in detail::pose_point_impl<>
116  inline double x() const /*!< Get X coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[0]; }
117  inline double y() const /*!< Get Y coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[1]; }
118 
119  inline double &x() /*!< Get ref to X coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[0]; }
120  inline double &y() /*!< Get ref to Y coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[1]; }
121 
122  inline void x(const double v) /*!< Set X coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]=v; }
123  inline void y(const double v) /*!< Set Y coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]=v; }
124 
125  inline void x_incr(const double v) /*!< X+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]+=v; }
126  inline void y_incr(const double v) /*!< Y+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]+=v; }
127 
128 
129  /** Return true for poses or points with a Z component, false otherwise. */
130  static inline bool is3DPoseOrPoint() { return DERIVEDCLASS::is_3D_val!=0; }
131 
132  /** Returns the squared euclidean distance to another pose/point: */
133  template <class OTHERCLASS> inline double sqrDistanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
134  {
135  if (b.is3DPoseOrPoint())
136  {
137  if (is3DPoseOrPoint())
138  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]);
139  else return square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
140  }
141  else
142  {
143  if (is3DPoseOrPoint())
144  return square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
145  else return square(x()-b.x()) + square(y()-b.y());
146  }
147  }
148 
149  /** Returns the Euclidean distance to another pose/point: */
150  template <class OTHERCLASS>
151  inline double distanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
152  {
153  return std::sqrt( sqrDistanceTo(b));
154  }
155 
156  /** Returns the squared 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
157  inline double distance2DToSquare( double ax, double ay ) const { return square(ax-x())+square(ay-y()); }
158 
159  /** Returns the squared 3D distance from this pose/point to a 3D point */
160  inline double distance3DToSquare( double ax, double ay, double az ) const {
161  return square(ax-x())+square(ay-y())+square(az-(is3DPoseOrPoint() ? static_cast<const DERIVEDCLASS*>(this)->m_coords[2] : 0) );
162  }
163 
164  /** Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
165  inline double distance2DTo( double ax, double ay ) const { return std::sqrt(distance2DToSquare(ax,ay)); }
166 
167  /** Returns the 3D distance from this pose/point to a 3D point */
168  inline double distance3DTo( double ax, double ay, double az ) const { return std::sqrt(distance3DToSquare(ax,ay,az)); }
169 
170  /** Returns the euclidean distance to a 3D point: */
171  inline double distanceTo(const mrpt::math::TPoint3D &b) const { return distance3DTo(b.x,b.y,b.z); }
172 
173  /** Returns the euclidean norm of vector: \f$ ||\mathbf{x}|| = \sqrt{x^2+y^2+z^2} \f$ */
174  inline double norm() const
175  {
176  return std::sqrt( square(x())+square(y())+ (!is3DPoseOrPoint() ? 0 : square(static_cast<const DERIVEDCLASS*>(this)->m_coords[2]) ) );
177  }
178 
179  /** Return the pose or point as a 1xN vector with all the components (see derived classes for each implementation) */
181  {
182  vector_double v;
183  static_cast<const DERIVEDCLASS*>(this)->getAsVector(v);
184  return v;
185  }
186 
187  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
188  * \sa getInverseHomogeneousMatrix
189  */
191  {
193  static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(m);
194  return m;
195  }
196 
197  /** Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose.
198  * \sa getHomogeneousMatrix
199  */
201  { // Get current HM & inverse in-place:
202  static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(out_HM);
204  }
205 
206  //! \overload
208  {
210  getInverseHomogeneousMatrix(M);
211  return M;
212  }
213 
214  /** @} */
215  }; // End of class def.
216 
217 
218  } // End of namespace
219 } // End of namespace
220 
221 #endif
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:116
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:151
void y_incr(const double v)
Definition: CPoseOrPoint.h:126
vector_double getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:180
static bool is3DPoseOrPoint()
Return true for poses or points with a Z component, false otherwise.
Definition: CPoseOrPoint.h:130
double distanceTo(const mrpt::math::TPoint3D &b) const
Returns the euclidean distance to a 3D point:
Definition: CPoseOrPoint.h:171
void getInverseHomogeneousMatrix(math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:200
This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrp...
double z
Z coordinate.
void y(const double v)
Definition: CPoseOrPoint.h:123
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
Definition: CPoseOrPoint.h:207
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
double sqrDistanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the squared euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:133
CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:190
void x_incr(const double v)
Definition: CPoseOrPoint.h:125
double x
X coordinate.
The base template class for 2D & 3D points and poses.
void x(const double v)
Definition: CPoseOrPoint.h:122
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
Definition: CPoseOrPoint.h:165
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:158
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double y
Y coordinate.
double distance3DToSquare(double ax, double ay, double az) const
Returns the squared 3D distance from this pose/point to a 3D point.
Definition: CPoseOrPoint.h:160
Lightweight 3D point.
double distance2DToSquare(double ax, double ay) const
Returns the squared 2D distance from this pose/point to a 2D point (ignores Z, if it exists)...
Definition: CPoseOrPoint.h:157
double distance3DTo(double ax, double ay, double az) const
Returns the 3D distance from this pose/point to a 3D point.
Definition: CPoseOrPoint.h:168



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo