Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
19namespace 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 {
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::utils::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 {
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 {
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
A numeric matrix of compile-time fixed size.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:108
static bool is3DPoseOrPoint()
Return true for poses or points with a Z component, false otherwise.
Definition: CPoseOrPoint.h:127
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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 distanceTo(const mrpt::math::TPoint3D &b) const
Returns the euclidean distance to a 3D point:
Definition: CPoseOrPoint.h:171
void y_incr(const double v)
Definition: CPoseOrPoint.h:123
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
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
void x_incr(const double v)
Definition: CPoseOrPoint.h:122
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
void x(const double v)
Definition: CPoseOrPoint.h:119
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
void y(const double v)
Definition: CPoseOrPoint.h:120
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
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:191
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
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPoseOrPoint.h:208
virtual void setToNaN()=0
Set all data fields to quiet NaN.
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
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...
@ UNINITIALIZED_POSE
Definition: CPoseOrPoint.h:35
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 3D point.
double z
X,Y,Z coordinates.



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:21:41 UTC 2021