Main MRPT website > C++ reference
MRPT logo
CPose2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPOSE2D_H
29 #define CPOSE2D_H
30 
31 #include <mrpt/poses/CPose.h>
32 
33 namespace mrpt
34 {
35 namespace poses
36 {
37  DEFINE_SERIALIZABLE_PRE( CPose2D )
38 
39  /** A class used to store a 2D pose.
40  * A class used to store a 2D pose, including the 2D coordinate
41  * point and a heading (phi) angle.
42  *
43  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
44  * to the <a href="http://www.mrpt.org/2D_3D_Geometry" >2D/3D Geometry tutorial</a> in the wiki.
45  *
46  * <div align=center>
47  * <img src="CPose2D.gif">
48  * </div>
49  *
50  * \sa CPoseOrPoint,CPoint2D
51  * \ingroup poses_grp
52  */
53  class BASE_IMPEXP CPose2D : public CPose<CPose2D>, public mrpt::utils::CSerializable
54  {
55  public:
56  // This must be added to any CSerializable derived class:
57  DEFINE_SERIALIZABLE( CPose2D )
58 
59  public:
60  mrpt::math::CArrayDouble<2> m_coords; //!< [x,y]
61 
62  protected:
63  double m_phi; //!< The orientation of the pose, in radians.
64 
65  public:
66  /** Default constructor (all coordinates to 0) */
67  CPose2D();
68 
69  /** Constructor from an initial value of the pose.*/
70  CPose2D(const double x,const double y,const double phi);
71 
72  /** Constructor from a CPoint2D object. */
73  CPose2D(const CPoint2D &);
74 
75  /** Aproximation!! Avoid its use, since information is lost. */
76  explicit CPose2D(const CPose3D &);
77 
78  /** Constructor from lightweight object. */
79  CPose2D(const mrpt::math::TPose2D &);
80 
81  /** Constructor from CPoint3D with information loss. */
82  explicit CPose2D(const CPoint3D &);
83 
84  /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
85  inline CPose2D(TConstructorFlags_Poses constructor_dummy_param) { }
86 
87  /** Get the phi angle of the 2D pose (in radians) */
88  inline const double &phi() const { return m_phi; }
89  //! \overload
90  inline double &phi() { return m_phi; }
91 
92  /** Set the phi angle of the 2D pose (in radians) */
93  inline void phi(double angle) { m_phi=angle; }
94 
95  inline void phi_incr(const double Aphi) { m_phi+=Aphi; } //!< 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  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
108  */
109  CPose2D operator + (const CPose2D& D) const ;
110 
111  /** Makes \f$ this = A \oplus B \f$
112  * \note A or B can be "this" without problems.
113  */
114  void composeFrom(const CPose2D &A, const CPose2D &B);
115 
116  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
117  */
118  CPose3D operator + (const CPose3D& D) const ;
119 
120  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator.
121  */
122  CPoint2D operator + (const CPoint2D& u) const ;
123 
124  /** 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. */
125  void composePoint(double lx,double ly,double &gx, double &gy) const;
126 
127  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator.
128  */
129  CPoint3D operator + (const CPoint3D& u) const ;
130 
131  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
132  * \note A or B can be "this" without problems.
133  * \sa composeFrom, composePoint
134  */
135  void inverseComposeFrom(const CPose2D& A, const CPose2D& B );
136 
137  /** Compute \f$ RET = this \oplus b \f$ */
138  inline CPose2D operator - (const CPose2D& b) const
139  {
140  CPose2D ret(UNINITIALIZED_POSE);
141  ret.inverseComposeFrom(*this,b);
142  return ret;
143  }
144 
145  /** Scalar sum of components: This is diferent from poses
146  * composition, which is implemented as "+" operators in "CPose" derived classes.
147  */
148  void AddComponents(CPose2D &p);
149 
150  /** Scalar multiplication.
151  */
152  void operator *=(const double s);
153 
154  /** Make \f$ this = this \oplus b \f$ */
155  inline CPose2D& operator += (const CPose2D& b)
156  {
157  composeFrom(*this,b);
158  return *this;
159  }
160 
161  /** Forces "phi" to be in the range [-pi,pi];
162  */
163  void normalizePhi();
164 
165  /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
166  * \sa fromString
167  */
168  void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x(),y(),RAD2DEG(m_phi)); }
169  inline std::string asString() const { std::string s; asString(s); return s; }
170 
171  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
172  * \sa asString
173  * \exception std::exception On invalid format
174  */
175  void fromString(const std::string &s) {
176  CMatrixDouble m;
177  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
178  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
179  x( m.get_unsafe(0,0) );
180  y( m.get_unsafe(0,1) );
181  phi( DEG2RAD(m.get_unsafe(0,2)) );
182  }
183 
184  inline const double &operator[](unsigned int i)const
185  {
186  switch(i)
187  {
188  case 0:return m_coords[0];
189  case 1:return m_coords[1];
190  case 2:return m_phi;
191  default:
192  throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
193  }
194  }
195  inline double &operator[](unsigned int i)
196  {
197  switch(i)
198  {
199  case 0:return m_coords[0];
200  case 1:return m_coords[1];
201  case 2:return m_phi;
202  default:
203  throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
204  }
205  }
206 
207  /** makes: this = p (+) this */
208  inline void changeCoordinatesReference( const CPose2D & p ) { composeFrom(p,CPose2D(*this)); }
209 
210  typedef CPose2D type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
211  enum { is_3D_val = 0 };
212  static inline bool is_3D() { return is_3D_val!=0; }
213  enum { rotation_dimensions = 2 };
214  enum { is_PDF_val = 0 };
215  static inline bool is_PDF() { return is_PDF_val!=0; }
216 
217  inline const type_value & getPoseMean() const { return *this; }
218  inline type_value & getPoseMean() { return *this; }
219 
220  /** @name STL-like methods and typedefs
221  @{ */
222  typedef double value_type; //!< The type of the elements
223  typedef double& reference;
224  typedef const double& const_reference;
225  typedef std::size_t size_type;
226  typedef std::ptrdiff_t difference_type;
227 
228  // size is constant
229  enum { static_size = 3 };
230  static inline size_type size() { return static_size; }
231  static inline bool empty() { return false; }
232  static inline size_type max_size() { return static_size; }
233  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))); }
234 
235  /** @} */
236 
237  }; // End of class def.
238 
239 
240  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose2D& p);
241 
242  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y phi) */
243  CPose2D BASE_IMPEXP operator -(const CPose2D &p);
244 
245  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.
246 
247  bool BASE_IMPEXP operator==(const CPose2D &p1,const CPose2D &p2);
248  bool BASE_IMPEXP operator!=(const CPose2D &p1,const CPose2D &p2);
249 
250  typedef mrpt::aligned_containers<CPose2D>::vector_t StdVector_CPose2D; //!< Eigen aligment-compatible container
251  typedef mrpt::aligned_containers<CPose2D>::deque_t StdDeque_CPose2D; //!< Eigen aligment-compatible container
252 
253  } // End of namespace
254 } // End of namespace
255 
256 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013