Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
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  // For use in some constructors (eg. CPose3D)
34  {
36  };
37 
38  /** The base template class for 2D & 3D points and poses.
39  * This class use the Curiously Recurring Template Pattern (CRTP) to define
40  * a set of common methods to all the children classes without the cost
41  * of virtual methods. Since most important methods are inline, they will be 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 tutorial</a> online.
46  *
47  *
48  * <center><h2>Introduction to 2D and 3D representation classes</h2></center>
49  * <hr>
50  * <p>
51  * There are two class of spatial representation classes:
52  * - Point: A point in the common mathematical sense, with no directional information.
53  * - 2D: A 2D point is represented just by its coordinates (x,y).
54  * - 3D: A 3D point is represented by its coordinates (x,y,z).
55  * - Pose: It is a point, plus a direction.
56  * - 2D: A 2D pose is a 2D point plus a single angle, the yaw or &#966; angle: the angle from the positive X angle.
57  * - 3D: A 3D point is a 3D point plus three orientation angles (More details above).
58  * </p>
59  * In the case for a 3D orientation many representation angles can be used (Euler angles,yaw/pitch/roll,...)
60  * but all of them can be handled by a 4x4 matrix called "Homogeneous Matrix". This matrix includes both, the
61  * translation and the orientation for a point or a pose, and it can be obtained using
62  * the method getHomogeneousMatrix() which is defined for any pose or point. Note that when the YPR angles are
63  * used to define a 3D orientation, these three values can not be extracted from the matrix again.<br><br>
64  *
65  * <b>Homogeneous matrices:</b> These are 4x4 matrices which can represent any translation or rotation in 2D & 3D.
66  * See the tutorial online for more details. *
67  *
68  * <b>Operators:</b> There are operators defined for the pose compounding \f$ \oplus \f$ and inverse pose
69  * compounding \f$ \ominus \f$ of poses and points. For example, let "a" and "b" be 2D or 3D poses. Then "a+b"
70  * returns the resulting pose of "moving b" from "a"; and "b-a" returns the pose of "b" as it is seen
71  * "from a". They can be mixed points and poses, being 2D or 3D, in these operators, with the following
72  * results: <br>
73  *
74  * <div align="center" >
75  * <pre>
76  * Does "a+b" return a Pose or a Point?
77  * +---------------------------------+
78  * | a \ b | Pose | Point |
79  * +----------+-----------+----------+
80  * | Pose | Pose | Point |
81  * | Point | Pose | Point |
82  * +---------------------------------+
83  *
84  * Does "a-b" return a Pose or a Point?
85  * +---------------------------------+
86  * | a \ b | Pose | Point |
87  * +----------+-----------+----------+
88  * | Pose | Pose | Pose |
89  * | Point | Point | Point |
90  * +---------------------------------+
91  *
92  * Does "a+b" and "a-b" return a 2D or 3D object?
93  * +-------------------------+
94  * | a \ b | 2D | 3D |
95  * +----------+--------------+
96  * | 2D | 2D | 3D |
97  * | 3D | 3D | 3D |
98  * +-------------------------+
99  *
100  * </pre>
101  * </div>
102  *
103  * \sa CPose,CPoint
104  * \ingroup poses_grp
105  */
106  template <class DERIVEDCLASS>
107  class CPoseOrPoint : public mrpt::poses::detail::pose_point_impl<DERIVEDCLASS, mrpt::poses::detail::T3DTypeHelper<DERIVEDCLASS>::is_3D_val>
108  {
109  public:
110  /** Common members of all points & poses classes.
111  @{ */
112  // Note: the access to "z" is implemented (only for 3D data types), in detail::pose_point_impl<>
113  inline double x() const /*!< Get X coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[0]; }
114  inline double y() const /*!< Get Y coord. */ { return static_cast<const DERIVEDCLASS*>(this)->m_coords[1]; }
115 
116  inline double &x() /*!< Get ref to X coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[0]; }
117  inline double &y() /*!< Get ref to Y coord. */ { return static_cast<DERIVEDCLASS*>(this)->m_coords[1]; }
118 
119  inline void x(const double v) /*!< Set X coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]=v; }
120  inline void y(const double v) /*!< Set Y coord. */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]=v; }
121 
122  inline void x_incr(const double v) /*!< X+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[0]+=v; }
123  inline void y_incr(const double v) /*!< Y+=v */ { static_cast<DERIVEDCLASS*>(this)->m_coords[1]+=v; }
124 
125 
126  /** Return true for poses or points with a Z component, false otherwise. */
127  static inline bool is3DPoseOrPoint() { return DERIVEDCLASS::is_3D_val!=0; }
128 
129  /** Returns the squared euclidean distance to another pose/point: */
130  template <class OTHERCLASS> inline double sqrDistanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
131  {
132  using mrpt::math::square;
133 
134  if (b.is3DPoseOrPoint())
135  {
136  if (is3DPoseOrPoint())
137  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]);
138  else return square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
139  }
140  else
141  {
142  if (is3DPoseOrPoint())
143  return square(x()-b.x()) + square(y()-b.y()) + square(static_cast<const OTHERCLASS*>(&b)->m_coords[2]);
144  else return square(x()-b.x()) + square(y()-b.y());
145  }
146  }
147 
148  /** Returns the Euclidean distance to another pose/point: */
149  template <class OTHERCLASS>
150  inline double distanceTo(const CPoseOrPoint<OTHERCLASS> &b) const
151  {
152  return std::sqrt( sqrDistanceTo(b));
153  }
154 
155  /** Returns the squared 2D distance from this pose/point to a 2D point (ignores Z, if it exists). */
156  inline double distance2DToSquare( double ax, double ay ) const { using mrpt::math::square; return square(ax-x())+square(ay-y()); }
157 
158  /** Returns the squared 3D distance from this pose/point to a 3D point */
159  inline double distance3DToSquare( double ax, double ay, double az ) const {
160  using mrpt::math::square;
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  using mrpt::math::square;
177  return std::sqrt( square(x())+square(y())+ (!is3DPoseOrPoint() ? 0 : square(static_cast<const DERIVEDCLASS*>(this)->m_coords[2]) ) );
178  }
179 
180  /** Return the pose or point as a 1xN vector with all the components (see derived classes for each implementation) */
182  {
184  static_cast<const DERIVEDCLASS*>(this)->getAsVector(v);
185  return v;
186  }
187 
188  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
189  * \sa getInverseHomogeneousMatrix
190  */
192  {
194  static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(m);
195  return m;
196  }
197 
198  /** Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose.
199  * \sa getHomogeneousMatrix
200  */
202  { // Get current HM & inverse in-place:
203  static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(out_HM);
205  }
206 
207  //! \overload
209  {
212  return M;
213  }
214 
215  /** Set all data fields to quiet NaN */
216  virtual void setToNaN() = 0;
217 
218  /** @} */
219  }; // End of class def.
220 
221 
222  } // End of namespace
223 } // End of namespace
224 
225 #endif
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
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:181
void y_incr(const double v)
Definition: CPoseOrPoint.h:123
static bool is3DPoseOrPoint()
Return true for poses or points with a Z component, false otherwise.
Definition: CPoseOrPoint.h:127
double distanceTo(const mrpt::math::TPoint3D &b) const
Returns the euclidean distance to a 3D point:
Definition: CPoseOrPoint.h:171
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:191
void y(const double v)
Definition: CPoseOrPoint.h:120
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
Definition: CPoseOrPoint.h:208
A numeric matrix of compile-time fixed size.
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:130
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:201
void x_incr(const double v)
Definition: CPoseOrPoint.h:122
GLubyte GLubyte b
Definition: glext.h:5575
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:107
void x(const double v)
Definition: CPoseOrPoint.h:119
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
const GLdouble * v
Definition: glext.h:3603
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...
T square(const T x)
Inline function for the square of a number.
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:159
Lightweight 3D point.
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:156
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019