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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019