Main MRPT website > C++ reference for MRPT 1.5.6
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-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 CPOSE2D_H
10 #define CPOSE2D_H
11 
12 #include <mrpt/poses/CPose.h>
14 
15 namespace mrpt
16 {
17 namespace poses
18 {
19  DEFINE_SERIALIZABLE_PRE( CPose2D )
20 
21  /** A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
22  * Use this class instead of lightweight mrpt::math::TPose2D when pose/point composition is to be called
23  * multiple times with the same pose, since this class caches calls to expensive trigronometric functions.
24  *
25  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
26  * to the <a href="http://www.mrpt.org/2D_3D_Geometry" >2D/3D Geometry tutorial</a> in the wiki.
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 on-manifold optimization", Jose-Luis Blanco. http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
33  * \sa CPoseOrPoint,CPoint2D
34  * \ingroup poses_grp
35  */
36  class BASE_IMPEXP CPose2D : public CPose<CPose2D>, public mrpt::utils::CSerializable
37  {
38  public:
39  // This must be added to any CSerializable derived class:
41 
42  public:
43  mrpt::math::CArrayDouble<2> m_coords; //!< [x,y]
44 
45  protected:
46  double m_phi; //!< The orientation of the pose, in radians.
47  mutable double m_cosphi, m_sinphi; //!< Precomputed cos() & sin() of phi.
48  mutable bool m_cossin_uptodate;
49 
50  inline void update_cached_cos_sin() const {
51  if (m_cossin_uptodate) return;
52 #ifdef HAVE_SINCOS
53  ::sincos(m_phi,&m_sinphi,&m_cosphi);
54 #else
55  m_cosphi=::cos(m_phi);
56  m_sinphi=::sin(m_phi);
57 #endif
58  m_cossin_uptodate=true;
59  }
60 
61  public:
62  /** Default constructor (all coordinates to 0) */
63  CPose2D();
64 
65  /** Constructor from an initial value of the pose.*/
66  CPose2D(const double x,const double y,const double phi);
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 
77  /** Constructor from CPoint3D with information loss. */
78  explicit CPose2D(const CPoint3D &);
79 
80  /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
81  inline CPose2D(TConstructorFlags_Poses ) : m_cossin_uptodate(false) { }
82 
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() { m_cossin_uptodate=false; return m_phi; } //-V659
87 
88  /** Get a (cached) value of cos(phi), recomputing it only once when phi changes. */
89  inline double phi_cos() const { update_cached_cos_sin(); return m_cosphi; }
90  /** Get a (cached) value of sin(phi), recomputing it only once when phi changes. */
91  inline double phi_sin() const { update_cached_cos_sin(); return m_sinphi; }
92 
93  /** Set the phi angle of the 2D pose (in radians) */
94  inline void phi(double angle) { m_phi=angle; m_cossin_uptodate=false; }
95 
96  inline void phi_incr(const double Aphi) { m_phi+=Aphi; m_cossin_uptodate=false; } //!< Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed)
97 
98  /** Returns a 1x3 vector with [x y phi] */
99  void getAsVector(mrpt::math::CVectorDouble &v) const;
100  /// \overload
101  void getAsVector(mrpt::math::CArrayDouble<3> &v) const;
102 
103  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
104  * \sa getInverseHomogeneousMatrix
105  */
106  void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 & out_HM ) const;
107 
108  void getRotationMatrix(mrpt::math::CMatrixDouble22 &R) const; //!< Returns the SE(2) 2x2 rotation matrix
109  void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const; //!< Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1.
110 
113  getRotationMatrix(R);
114  return R;
115  }
116 
117  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator. */
118  CPose2D operator + (const CPose2D& D) const ;
119 
120  /** Makes \f$ this = A \oplus B \f$
121  * \note A or B can be "this" without problems. */
122  void composeFrom(const CPose2D &A, const CPose2D &B);
123 
124  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator. */
125  CPose3D operator + (const CPose3D& D) const ;
126 
127  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator. */
128  CPoint2D operator + (const CPoint2D& u) const ;
129 
130  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 2D points and P this 2D pose. */
131  void composePoint(double lx,double ly,double &gx, double &gy) const;
132 
133  /** overload \f$ G = P \oplus L \f$ with G and L being 2D points and P this 2D pose */
134  void composePoint(const mrpt::math::TPoint2D &l, mrpt::math::TPoint2D &g) const;
135 
136  /** overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this 2D pose (the "z" coordinate remains unmodified) */
137  void composePoint(const mrpt::math::TPoint3D &l, mrpt::math::TPoint3D &g) const;
138  /** overload (the "z" coordinate remains unmodified) */
139  void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz) const;
140 
141  /** Computes the 2D point L such as \f$ L = G \ominus this \f$. \sa composePoint, composeFrom */
142  void inverseComposePoint(const double gx,const double gy, double &lx,double &ly) const;
143  /** \overload */
145  inverseComposePoint(g.x,g.y, l.x,l.y);
146  }
147 
148  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator. */
149  CPoint3D operator + (const CPoint3D& u) const ;
150 
151  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
152  * \note A or B can be "this" without problems.
153  * \sa composeFrom, composePoint
154  */
155  void inverseComposeFrom(const CPose2D& A, const CPose2D& B );
156 
157  /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
158  void inverse();
159 
160  /** Compute \f$ RET = this \ominus b \f$ */
161  inline CPose2D operator - (const CPose2D& b) const
162  {
163  CPose2D ret(UNINITIALIZED_POSE);
164  ret.inverseComposeFrom(*this,b);
165  return ret;
166  }
167 
168  /** The operator \f$ a \ominus b \f$ is the pose inverse compounding operator. */
169  CPose3D operator -(const CPose3D& b) const;
170 
171 
172  /** Scalar sum of components: This is diferent from poses
173  * composition, which is implemented as "+" operators in "CPose" derived classes.
174  */
175  void AddComponents(const CPose2D &p);
176 
177  /** Scalar multiplication.
178  */
179  void operator *=(const double s);
180 
181  /** Make \f$ this = this \oplus b \f$ */
182  CPose2D& operator += (const CPose2D& b);
183 
184  /** Forces "phi" to be in the range [-pi,pi];
185  */
186  void normalizePhi();
187 
188  /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
189  * \sa fromString
190  */
191  void asString(std::string &s) const;
192  inline std::string asString() const { std::string s; asString(s); return s; }
193 
194  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
195  * \sa asString
196  * \exception std::exception On invalid format
197  */
198  void fromString(const std::string &s);
199 
200  inline const double &operator[](unsigned int i)const
201  {
202  switch(i)
203  {
204  case 0:return m_coords[0];
205  case 1:return m_coords[1];
206  case 2:return m_phi;
207  default:
208  throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
209  }
210  }
211  inline double &operator[](unsigned int i)
212  {
213  switch(i)
214  {
215  case 0:return m_coords[0];
216  case 1:return m_coords[1];
217  case 2:return m_phi;
218  default:
219  throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
220  }
221  }
222 
223  /** makes: this = p (+) this */
224  inline void changeCoordinatesReference( const CPose2D & p ) { composeFrom(p,CPose2D(*this)); }
225 
226  /** Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance. */
227  double distance2DFrobeniusTo( const CPose2D & p) const;
228 
229  typedef CPose2D type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
230  enum { is_3D_val = 0 };
231  static inline bool is_3D() { return is_3D_val!=0; }
232  enum { rotation_dimensions = 2 };
233  enum { is_PDF_val = 0 };
234  static inline bool is_PDF() { return is_PDF_val!=0; }
235 
236  inline const type_value & getPoseMean() const { return *this; }
237  inline type_value & getPoseMean() { return *this; }
238 
239  void setToNaN() MRPT_OVERRIDE;
240 
241  /** @name STL-like methods and typedefs
242  @{ */
243  typedef double value_type; //!< The type of the elements
244  typedef double& reference;
245  typedef const double& const_reference;
246  typedef std::size_t size_type;
248 
249  // size is constant
250  enum { static_size = 3 };
251  static inline size_type size() { return static_size; }
252  static inline bool empty() { return false; }
253  static inline size_type max_size() { return static_size; }
254  static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose2D to %u.",static_cast<unsigned>(n))); }
255 
256  /** @} */
257 
258  }; // End of class def.
259  DEFINE_SERIALIZABLE_POST( CPose2D )
260 
261 
262  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose2D& p);
263 
264  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y phi) \sa CPose2D::inverse() */
265  CPose2D BASE_IMPEXP operator -(const CPose2D &p);
266 
267  mrpt::math::TPoint2D BASE_IMPEXP operator +(const CPose2D &pose, const mrpt::math::TPoint2D &pnt); //!< Compose a 2D point from a new coordinate base given by a 2D pose.
268 
269  bool BASE_IMPEXP operator==(const CPose2D &p1,const CPose2D &p2);
270  bool BASE_IMPEXP operator!=(const CPose2D &p1,const CPose2D &p2);
271 
272  typedef mrpt::aligned_containers<CPose2D>::vector_t StdVector_CPose2D; //!< Eigen aligment-compatible container
273  typedef mrpt::aligned_containers<CPose2D>::deque_t StdDeque_CPose2D; //!< Eigen aligment-compatible container
274 
275  } // End of namespace
276 } // End of namespace
277 
278 #endif
const type_value & getPoseMean() const
Definition: CPose2D.h:236
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
const double & operator[](unsigned int i) const
Definition: CPose2D.h:200
void changeCoordinatesReference(const CPose2D &p)
makes: this = p (+) this
Definition: CPose2D.h:224
std::string asString() const
Definition: CPose2D.h:192
double y
X,Y coordinates.
std::size_t size_type
Definition: CPose2D.h:246
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l) const
Definition: CPose2D.h:144
GLboolean GLboolean g
Definition: glew.h:5406
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
mrpt::math::TPoint2D BASE_IMPEXP 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:359
type_value & getPoseMean()
Definition: CPose2D.h:237
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
Helper types for STL containers with Eigen memory allocators.
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:89
CPose2D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
Definition: CPose2D.h:81
GLdouble l
Definition: glew.h:5092
double value_type
The type of the elements.
Definition: CPose2D.h:243
GLdouble s
Definition: glew.h:1295
A numeric matrix of compile-time fixed size.
GLsizei n
Definition: glew.h:5051
mrpt::math::CMatrixDouble22 getRotationMatrix() const
Definition: CPose2D.h:111
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
CPose2D BASE_IMPEXP 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:307
static bool is_3D()
Definition: CPose2D.h:231
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:25
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
mrpt::aligned_containers< CPose2D >::deque_t StdDeque_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:273
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLfloat GLfloat p
Definition: glew.h:10113
CPose2D type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Definition: CPose2D.h:229
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
_W64 int ptrdiff_t
Definition: glew.h:133
static bool is_PDF()
Definition: CPose2D.h:234
#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:211
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:72
static size_type max_size()
Definition: CPose2D.h:253
static void resize(const size_t n)
Definition: CPose2D.h:254
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:91
Lightweight 2D pose.
#define DEFINE_SERIALIZABLE_POST(class_name)
void phi(double angle)
Set the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:94
GLdouble angle
Definition: glew.h:5105
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
double & reference
Definition: CPose2D.h:244
Lightweight 3D point.
const double & const_reference
Definition: CPose2D.h:245
std::ptrdiff_t difference_type
Definition: CPose2D.h:247
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:233
static size_type size()
Definition: CPose2D.h:251
Lightweight 2D point.
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:272
void phi_incr(const double Aphi)
Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed) ...
Definition: CPose2D.h:96
static bool empty()
Definition: CPose2D.h:252



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018