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



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo