MRPT  2.0.1
CPoseOrPoint.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/CVectorFixed.h>
12 #include <mrpt/math/TPoint3D.h>
16 
17 namespace mrpt
18 {
19 /** \defgroup poses_grp 2D/3D points and poses
20  * \ingroup mrpt_poses_grp */
21 
22 /** \defgroup poses_pdf_grp 2D/3D point and pose PDFs
23  * \ingroup mrpt_poses_grp */
24 
25 /** Classes for 2D/3D geometry representation, both of single values and
26  * probability density distributions (PDFs) in many forms.
27  * \ingroup poses_grp poses_pdf_grp
28  */
29 namespace poses
30 {
31 // For use in some constructors (eg. CPose3D)
33 {
35 };
36 
37 /** The base template class for 2D & 3D points and poses.
38  * This class use the Curiously Recurring Template Pattern (CRTP) to define
39  * a set of common methods to all the children classes without the cost
40  * of virtual methods. Since most important methods are inline, they will be
41  * expanded
42  * at compile time and optimized for every specific derived case.
43  *
44  * For more information and examples, refer
45  * to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry
46  * tutorial</a> online.
47  *
48  *
49  * <center><h2>Introduction to 2D and 3D representation classes</h2></center>
50  * <hr>
51  * <p>
52  * There are two class of spatial representation classes:
53  * - Point: A point in the common mathematical sense, with no directional
54  * information.
55  * - 2D: A 2D point is represented just by its coordinates (x,y).
56  * - 3D: A 3D point is represented by its coordinates (x,y,z).
57  * - Pose: It is a point, plus a direction.
58  * - 2D: A 2D pose is a 2D point plus a single angle, the yaw or &#966;
59  * angle:
60  * the angle from the positive X angle.
61  * - 3D: A 3D point is a 3D point plus three orientation angles (More
62  * details
63  * above).
64  * </p>
65  * In the case for a 3D orientation many representation angles can be used
66  * (Euler angles,yaw/pitch/roll,...)
67  * but all of them can be handled by a 4x4 matrix called "Homogeneous Matrix".
68  * This matrix includes both, the
69  * translation and the orientation for a point or a pose, and it can be
70  * obtained using
71  * the method getHomogeneousMatrix() which is defined for any pose or point.
72  * Note that when the YPR angles are
73  * used to define a 3D orientation, these three values can not be extracted
74  * from the matrix again.<br><br>
75  *
76  * <b>Homogeneous matrices:</b> These are 4x4 matrices which can represent any
77  * translation or rotation in 2D & 3D.
78  * See the tutorial online for more details. *
79  *
80  * <b>Operators:</b> There are operators defined for the pose compounding \f$
81  * \oplus \f$ and inverse pose
82  * compounding \f$ \ominus \f$ of poses and points. For example, let "a" and
83  * "b" be 2D or 3D poses. Then "a+b"
84  * returns the resulting pose of "moving b" from "a"; and "b-a" returns the
85  * pose of "b" as it is seen
86  * "from a". They can be mixed points and poses, being 2D or 3D, in these
87  * operators, with the following
88  * results: <br>
89  *
90  * <div align="center" >
91  * <pre>
92  * Does "a+b" return a Pose or a Point?
93  * +---------------------------------+
94  * | a \ b | Pose | Point |
95  * +----------+-----------+----------+
96  * | Pose | Pose | Point |
97  * | Point | Pose | Point |
98  * +---------------------------------+
99  *
100  * Does "a-b" return a Pose or a Point?
101  * +---------------------------------+
102  * | a \ b | Pose | Point |
103  * +----------+-----------+----------+
104  * | Pose | Pose | Pose |
105  * | Point | Point | Point |
106  * +---------------------------------+
107  *
108  * Does "a+b" and "a-b" return a 2D or 3D object?
109  * +-------------------------+
110  * | a \ b | 2D | 3D |
111  * +----------+--------------+
112  * | 2D | 2D | 3D |
113  * | 3D | 3D | 3D |
114  * +-------------------------+
115  *
116  * </pre>
117  * </div>
118  *
119  * \sa CPose,CPoint
120  * \ingroup poses_grp
121  */
122 template <class DERIVEDCLASS, std::size_t DIM>
125  DERIVEDCLASS,
126  mrpt::poses::detail::T3DTypeHelper<DERIVEDCLASS>::is_3D_val>
127 {
128  public:
129  const DERIVEDCLASS& derived() const
130  {
131  return *static_cast<const DERIVEDCLASS*>(this);
132  }
133  DERIVEDCLASS& derived() { return *static_cast<DERIVEDCLASS*>(this); }
134 
135  /** Fixed-size vector of the correct size to hold all the coordinates of the
136  * point/pose */
138 
139  /** Common members of all points & poses classes.
140  @{ */
141  // Note: the access to "z" is implemented (only for 3D data types), in
142  // detail::pose_point_impl<>
143  inline double x() const /*!< Get X coord. */
144  {
145  return derived().m_coords[0];
146  }
147  inline double y() const /*!< Get Y coord. */
148  {
149  return derived().m_coords[1];
150  }
151 
152  inline double& x() /*!< Get ref to X coord. */
153  {
154  return derived().m_coords[0];
155  }
156  inline double& y() /*!< Get ref to Y coord. */
157  {
158  return derived().m_coords[1];
159  }
160 
161  inline void x(const double v) /*!< Set X coord. */
162  {
163  derived().m_coords[0] = v;
164  }
165  inline void y(const double v) /*!< Set Y coord. */
166  {
167  derived().m_coords[1] = v;
168  }
169 
170  inline void x_incr(const double v) /*!< X+=v */
171  {
172  derived().m_coords[0] += v;
173  }
174  inline void y_incr(const double v) /*!< Y+=v */
175  {
176  derived().m_coords[1] += v;
177  }
178 
179  /** Return true for poses or points with a Z component, false otherwise. */
180  static inline bool is3DPoseOrPoint()
181  {
182  return DERIVEDCLASS::is_3D_val != 0;
183  }
184 
185  /** Returns the squared euclidean distance to another pose/point: */
186  template <class OTHERCLASS, std::size_t DIM2>
187  inline double sqrDistanceTo(const CPoseOrPoint<OTHERCLASS, DIM2>& b) const
188  {
189  using mrpt::square;
190 
191  if (b.is3DPoseOrPoint())
192  {
193  if (is3DPoseOrPoint())
194  return square(x() - b.x()) + square(y() - b.y()) +
195  square(
196  derived().m_coords[2] -
197  static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
198  else
199  return square(x() - b.x()) + square(y() - b.y()) +
200  square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
201  }
202  else
203  {
204  if (is3DPoseOrPoint())
205  return square(x() - b.x()) + square(y() - b.y()) +
206  square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
207  else
208  return square(x() - b.x()) + square(y() - b.y());
209  }
210  }
211 
212  /** Returns the Euclidean distance to another pose/point: */
213  template <class OTHERCLASS, std::size_t DIM2>
214  inline double distanceTo(const CPoseOrPoint<OTHERCLASS, DIM2>& b) const
215  {
216  return std::sqrt(sqrDistanceTo(b));
217  }
218 
219  /** Returns the squared 2D distance from this pose/point to a 2D point
220  * (ignores Z, if it exists). */
221  inline double distance2DToSquare(double ax, double ay) const
222  {
223  using mrpt::square;
224  return square(ax - x()) + square(ay - y());
225  }
226 
227  /** Returns the squared 3D distance from this pose/point to a 3D point */
228  inline double distance3DToSquare(double ax, double ay, double az) const
229  {
230  using mrpt::square;
231  return square(ax - x()) + square(ay - y()) +
232  square(az - (is3DPoseOrPoint() ? derived().m_coords[2] : 0));
233  }
234 
235  /** Returns the 2D distance from this pose/point to a 2D point (ignores Z,
236  * if it exists). */
237  inline double distance2DTo(double ax, double ay) const
238  {
239  return std::sqrt(distance2DToSquare(ax, ay));
240  }
241 
242  /** Returns the 3D distance from this pose/point to a 3D point */
243  inline double distance3DTo(double ax, double ay, double az) const
244  {
245  return std::sqrt(distance3DToSquare(ax, ay, az));
246  }
247 
248  /** Returns the euclidean distance to a 3D point: */
249  inline double distanceTo(const mrpt::math::TPoint3D& b) const
250  {
251  return distance3DTo(b.x, b.y, b.z);
252  }
253 
254  /** Returns the euclidean norm of vector: \f$ ||\mathbf{x}|| =
255  * \sqrt{x^2+y^2+z^2} \f$ */
256  inline double norm() const
257  {
258  using mrpt::square;
259  return std::sqrt(
260  square(x()) + square(y()) +
261  (!is3DPoseOrPoint() ? 0 : square(derived().m_coords[2])));
262  }
263 
264  /** Return the pose or point as a 1xN vector with all the components (see
265  * derived classes for each implementation) */
267  {
268  vector_t v;
269  derived().asVector(v);
270  return v;
271  }
272 
273  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
274  * point(translation) or pose (translation+orientation).
275  * \sa getInverseHomogeneousMatrix
276  */
277  template <class MATRIX44>
278  inline MATRIX44 getHomogeneousMatrixVal() const
279  {
280  MATRIX44 m;
281  derived().getHomogeneousMatrix(m);
282  return m;
283  }
284 
285  /** Returns the corresponding 4x4 inverse homogeneous transformation matrix
286  * for this point or pose.
287  * \sa getHomogeneousMatrix
288  */
289  template <class MATRIX44>
290  inline void getInverseHomogeneousMatrix(MATRIX44& out_HM) const
291  { // Get current HM & inverse in-place:
292  derived().getHomogeneousMatrix(out_HM);
294  }
295 
296  //! \overload
297  template <class MATRIX44>
298  inline MATRIX44 getInverseHomogeneousMatrixVal() const
299  {
300  MATRIX44 M;
302  return M;
303  }
304 
305  /** Set all data fields to quiet NaN */
306  virtual void setToNaN() = 0;
307 
308  /** @} */
309 }; // End of class def.
310 
311 } // namespace poses
312 } // namespace mrpt
DERIVEDCLASS & derived()
Definition: CPoseOrPoint.h:133
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
const DERIVEDCLASS & derived() const
Definition: CPoseOrPoint.h:129
double distanceTo(const mrpt::math::TPoint3D &b) const
Returns the euclidean distance to a 3D point:
Definition: CPoseOrPoint.h:249
MATRIX44 getInverseHomogeneousMatrixVal() const
Definition: CPoseOrPoint.h:298
void y(const double v)
Definition: CPoseOrPoint.h:165
double sqrDistanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the squared euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:187
double distance3DTo(double ax, double ay, double az) const
Returns the 3D distance from this pose/point to a 3D point.
Definition: CPoseOrPoint.h:243
void x(const double v)
Definition: CPoseOrPoint.h:161
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:228
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
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:221
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:290
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void y_incr(const double v)
Definition: CPoseOrPoint.h:174
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:123
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
static bool is3DPoseOrPoint()
Return true for poses or points with a Z component, false otherwise.
Definition: CPoseOrPoint.h:180
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void x_incr(const double v)
Definition: CPoseOrPoint.h:170
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...
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:237
virtual void setToNaN()=0
Set all data fields to quiet NaN.
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020