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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020