MRPT  1.9.9
CPose2D.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 
11 #include <mrpt/poses/CPose.h>
13 
14 namespace mrpt::poses
15 {
16 class CPose3D;
17 /** A class used to store a 2D pose, including the 2D coordinate point and a
18  * heading (phi) angle.
19  * Use this class instead of lightweight mrpt::math::TPose2D when pose/point
20  * composition is to be called
21  * multiple times with the same pose, since this class caches calls to
22  * expensive trigronometric functions.
23  *
24  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint,
25  * or refer to [this documentation page]
26  *(http://www.mrpt.org/tutorials/programming/maths-and-geometry/2d_3d_geometry/)
27  *
28  * <div align=center>
29  * <img src="CPose2D.gif">
30  * </div>
31  *
32  * \note Read also: "A tutorial on SE(3) transformation parameterizations and
33  * on-manifold optimization", Jose-Luis Blanco.
34  * http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
35  * \sa CPoseOrPoint,CPoint2D
36  * \ingroup poses_grp
37  */
38 class CPose2D : public CPose<CPose2D>, public mrpt::serialization::CSerializable
39 {
40  public:
43 
44  public:
45  /** [x,y] */
47 
48  protected:
49  /** The orientation of the pose, in radians. */
50  double m_phi{0};
51  /** Precomputed cos() & sin() of phi. */
52  mutable double m_cosphi, m_sinphi;
53  mutable bool m_cossin_uptodate{false};
54  void update_cached_cos_sin() const;
55 
56  public:
57  /** Default constructor (all coordinates to 0) */
58  CPose2D();
59 
60  /** Constructor from an initial value of the pose.*/
61  CPose2D(const double x, const double y, const double phi);
62 
63  /** Returns the identity transformation */
64  static CPose2D Identity() { return CPose2D(); }
65 
66  /** Constructor from a CPoint2D object. */
67  explicit CPose2D(const CPoint2D&);
68 
69  /** Aproximation!! Avoid its use, since information is lost. */
70  explicit CPose2D(const CPose3D&);
71 
72  /** Constructor from lightweight object. */
73  explicit CPose2D(const mrpt::math::TPose2D&);
74 
76 
77  /** Constructor from CPoint3D with information loss. */
78  explicit CPose2D(const CPoint3D&);
79 
80  /** Fast constructor that leaves all the data uninitialized - call with
81  * UNINITIALIZED_POSE as argument */
83  /** Get the phi angle of the 2D pose (in radians) */
84  inline const double& phi() const { return m_phi; }
85  //! \overload
86  inline double& phi()
87  {
88  m_cossin_uptodate = false;
89  return m_phi;
90  } //-V659
91 
92  /** Get a (cached) value of cos(phi), recomputing it only once when phi
93  * changes. */
94  inline double phi_cos() const
95  {
97  return m_cosphi;
98  }
99  /** Get a (cached) value of sin(phi), recomputing it only once when phi
100  * changes. */
101  inline double phi_sin() const
102  {
104  return m_sinphi;
105  }
106 
107  /** Set the phi angle of the 2D pose (in radians) */
108  inline void phi(double angle)
109  {
110  m_phi = angle;
111  m_cossin_uptodate = false;
112  }
113 
114  /** Increment the PHI angle (without checking the 2 PI range, call
115  * normalizePhi is needed) */
116  inline void phi_incr(const double Aphi)
117  {
118  m_phi += Aphi;
119  m_cossin_uptodate = false;
120  }
121 
122  /** Returns a 1x3 vector with [x y phi] */
124  /// \overload
126 
127  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
128  * point(translation) or pose (translation+orientation).
129  * \sa getInverseHomogeneousMatrix
130  */
132 
133  /** Returns the SE(2) 2x2 rotation matrix */
135  /** Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1. */
137 
138  template <class MATRIX22>
139  inline MATRIX22 getRotationMatrix() const
140  {
141  MATRIX22 R;
143  return R;
144  }
145 
146  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
147  */
148  CPose2D operator+(const CPose2D& D) const;
149 
150  /** Makes \f$ this = A \oplus B \f$
151  * \note A or B can be "this" without problems. */
152  void composeFrom(const CPose2D& A, const CPose2D& B);
153 
154  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
155  */
156  CPose3D operator+(const CPose3D& D) const;
157 
158  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding
159  * operator. */
160  CPoint2D operator+(const CPoint2D& u) const;
161 
162  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
163  * \f$ with G and L being 2D points and P this 2D pose. */
164  void composePoint(double lx, double ly, double& gx, double& gy) const;
165 
166  /** overload \f$ G = P \oplus L \f$ with G and L being 2D points and P this
167  * 2D pose */
168  void composePoint(
169  const mrpt::math::TPoint2D& l, mrpt::math::TPoint2D& g) const;
170 
171  /** overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this
172  * 2D pose (the "z" coordinate remains unmodified) */
173  void composePoint(
174  const mrpt::math::TPoint3D& l, mrpt::math::TPoint3D& g) const;
175  /** overload (the "z" coordinate remains unmodified) */
176  void composePoint(
177  double lx, double ly, double lz, double& gx, double& gy,
178  double& gz) const;
179 
180  /** Computes the 2D point L such as \f$ L = G \ominus this \f$. \sa
181  * composePoint, composeFrom */
182  void inverseComposePoint(
183  const double gx, const double gy, double& lx, double& ly) const;
184  /** \overload */
185  inline void inverseComposePoint(
187  {
188  inverseComposePoint(g.x, g.y, l.x, l.y);
189  }
190 
191  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding
192  * operator. */
193  CPoint3D operator+(const CPoint3D& u) const;
194 
195  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
196  * than "this= A - B;" since it avoids the temporary object.
197  * \note A or B can be "this" without problems.
198  * \sa composeFrom, composePoint
199  */
200  void inverseComposeFrom(const CPose2D& A, const CPose2D& B);
201 
202  /** Convert this pose into its inverse, saving the result in itself. \sa
203  * operator- */
204  void inverse();
205 
206  /** Compute \f$ RET = this \ominus b \f$ */
207  inline CPose2D operator-(const CPose2D& b) const
208  {
210  ret.inverseComposeFrom(*this, b);
211  return ret;
212  }
213 
214  /** The operator \f$ a \ominus b \f$ is the pose inverse compounding
215  * operator. */
216  CPose3D operator-(const CPose3D& b) const;
217 
218  /** Scalar sum of components: This is diferent from poses
219  * composition, which is implemented as "+" operators in "CPose" derived
220  * classes.
221  */
222  void AddComponents(const CPose2D& p);
223 
224  /** Scalar multiplication.
225  */
226  void operator*=(const double s);
227 
228  /** Make \f$ this = this \oplus b \f$ */
229  CPose2D& operator+=(const CPose2D& b);
230 
231  /** Forces "phi" to be in the range [-pi,pi];
232  */
233  void normalizePhi();
234 
235  /** Return the opposite of the current pose instance by taking the negative
236  * of all its components \a individually
237  */
238  CPose2D getOppositeScalar() const;
239 
240  /** Returns a human-readable textual representation of the object (eg: "[x y
241  * yaw]", yaw in degrees)
242  * \sa fromString
243  */
244  void asString(std::string& s) const;
245  inline std::string asString() const
246  {
247  std::string s;
248  asString(s);
249  return s;
250  }
251 
252  /** Set the current object value from a string generated by 'asString' (eg:
253  * "[0.02 1.04 -0.8]" )
254  * \sa asString
255  * \exception std::exception On invalid format
256  */
257  void fromString(const std::string& s);
258  /** Same as fromString, but without requiring the square brackets in the
259  * string */
260  void fromStringRaw(const std::string& s);
261 
262  inline const double& operator[](unsigned int i) const
263  {
264  switch (i)
265  {
266  case 0:
267  return m_coords[0];
268  case 1:
269  return m_coords[1];
270  case 2:
271  return m_phi;
272  default:
273  throw std::runtime_error(
274  "CPose2D::operator[]: Index of bounds.");
275  }
276  }
277  inline double& operator[](unsigned int i)
278  {
279  switch (i)
280  {
281  case 0:
282  return m_coords[0];
283  case 1:
284  return m_coords[1];
285  case 2:
286  return m_phi;
287  default:
288  throw std::runtime_error(
289  "CPose2D::operator[]: Index of bounds.");
290  }
291  }
292 
293  /** makes: this = p (+) this */
295  {
296  composeFrom(p, CPose2D(*this));
297  }
298 
299  /** Returns the 2D distance from this pose/point to a 2D pose using the
300  * Frobenius distance. */
301  double distance2DFrobeniusTo(const CPose2D& p) const;
302 
303  /** Used to emulate CPosePDF types, for example, in
304  * mrpt::graphs::CNetworkOfPoses */
306  enum
307  {
309  };
310  static constexpr bool is_3D() { return is_3D_val != 0; }
311  enum
312  {
314  };
315  enum
316  {
318  };
319  static constexpr bool is_PDF() { return is_PDF_val != 0; }
320  inline const type_value& getPoseMean() const { return *this; }
321  inline type_value& getPoseMean() { return *this; }
322  void setToNaN() override;
323 
324  /** @name STL-like methods and typedefs
325  @{ */
326  /** The type of the elements */
327  using value_type = double;
328  using reference = double&;
329  using const_reference = const double&;
330  using size_type = std::size_t;
332 
333  // size is constant
334  enum
335  {
337  };
338  static constexpr size_type size() { return static_size; }
339  static constexpr bool empty() { return false; }
340  static constexpr size_type max_size() { return static_size; }
341  static inline void resize(const size_t n)
342  {
343  if (n != static_size)
344  throw std::logic_error(format(
345  "Try to change the size of CPose2D to %u.",
346  static_cast<unsigned>(n)));
347  }
348 
349  /** @} */
350 
351 }; // End of class def.
352 
353 std::ostream& operator<<(std::ostream& o, const CPose2D& p);
354 
355 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
356  * than a pose with negative x y phi) \sa CPose2D::inverse() */
357 CPose2D operator-(const CPose2D& p);
358 
359 /** Compose a 2D point from a new coordinate base given by a 2D pose. */
361  const CPose2D& pose, const mrpt::math::TPoint2D& pnt);
362 
363 bool operator==(const CPose2D& p1, const CPose2D& p2);
364 bool operator!=(const CPose2D& p1, const CPose2D& p2);
365 
366 } // namespace mrpt::poses
void changeCoordinatesReference(const CPose2D &p)
makes: this = p (+) this
Definition: CPose2D.h:294
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:101
double x
X,Y coordinates.
const type_value & getPoseMean() const
Definition: CPose2D.h:320
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Definition: CPose2D.cpp:386
GLenum GLsizei n
Definition: glext.h:5136
type_value & getPoseMean()
Definition: CPose2D.h:321
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:45
void operator*=(const double s)
Scalar multiplication.
Definition: CPose2D.cpp:284
void AddComponents(const CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
Definition: CPose2D.cpp:273
static CPose2D Identity()
Returns the identity transformation.
Definition: CPose2D.h:64
mrpt::math::CArrayDouble< 2 > m_coords
[x,y]
Definition: CPose2D.h:46
double m_cosphi
Precomputed cos() & sin() of phi.
Definition: CPose2D.h:52
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
GLdouble s
Definition: glext.h:3682
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:197
double & reference
Definition: CPose2D.h:328
CPose2D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
Definition: CPose2D.h:82
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:463
CPose2D operator-(const CPose2D &b) const
Compute .
Definition: CPose2D.h:207
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose2D.cpp:412
double value_type
The type of the elements.
Definition: CPose2D.h:327
A numeric matrix of compile-time fixed size.
static constexpr size_type size()
Definition: CPose2D.h:338
CArrayNumeric< double, N > CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:64
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:171
double m_phi
The orientation of the pose, in radians.
Definition: CPose2D.h:50
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:337
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose2D.cpp:445
GLubyte g
Definition: glext.h:6372
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:24
GLubyte GLubyte b
Definition: glext.h:6372
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose2D.cpp:346
static constexpr size_type max_size()
Definition: CPose2D.h:340
static constexpr bool is_PDF()
Definition: CPose2D.h:319
CPose2D operator+(const CPose2D &D) const
The operator is the pose compounding operator.
Definition: CPose2D.cpp:120
GLsizei const GLchar ** string
Definition: glext.h:4116
A class used to store a 2D point.
Definition: CPoint2D.h:32
A class used to store a 3D point.
Definition: CPoint3D.h:30
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
_W64 int ptrdiff_t
Definition: glew.h:136
MATRIX22 getRotationMatrix() const
Definition: CPose2D.h:139
std::string asString() const
Definition: CPose2D.h:245
std::size_t size_type
Definition: CPose2D.h:330
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
double & operator[](unsigned int i)
Definition: CPose2D.h:277
static constexpr bool is_3D()
Definition: CPose2D.h:310
const double & const_reference
Definition: CPose2D.h:329
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:163
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const float R
double & phi()
Definition: CPose2D.h:86
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
CPose2D & operator+=(const CPose2D &b)
Make .
Definition: CPose2D.cpp:395
CPose2D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose2D.cpp:435
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose2D.cpp:297
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
static void resize(const size_t n)
Definition: CPose2D.h:341
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:133
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:94
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:228
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
Definition: CPose2D.cpp:401
Lightweight 2D pose.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
GLenum GLint GLint y
Definition: glext.h:3542
void phi(double angle)
Set the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:108
double distance2DFrobeniusTo(const CPose2D &p) const
Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance.
Definition: CPose2D.cpp:417
std::ptrdiff_t difference_type
Definition: CPose2D.h:331
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
void inverseComposeFrom(const CPose2D &A, const CPose2D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
Definition: CPose2D.cpp:256
bool m_cossin_uptodate
Definition: CPose2D.h:53
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x3 vector with [x y phi].
Definition: CPose2D.cpp:361
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:6398
const double & operator[](unsigned int i) const
Definition: CPose2D.h:262
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l) const
Definition: CPose2D.h:185
CPose2D()
Default constructor (all coordinates to 0)
Definition: CPose2D.cpp:31
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:314
static constexpr bool empty()
Definition: CPose2D.h:339
void update_cached_cos_sin() const
Definition: CPose2D.cpp:451
void phi_incr(const double Aphi)
Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed) ...
Definition: CPose2D.h:116
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:137



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