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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019