Main MRPT website > C++ reference
MRPT logo

CPose2D.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPOSE2D_H
00029 #define CPOSE2D_H
00030 
00031 #include <mrpt/poses/CPose.h>
00032 
00033 namespace mrpt
00034 {
00035 namespace poses
00036 {
00037         DEFINE_SERIALIZABLE_PRE( CPose2D )
00038 
00039         /** A class used to store a 2D pose.
00040          *    A class used to store a 2D pose, including the 2D coordinate
00041          *      point and a heading (phi) angle.
00042          *
00043          *  For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
00044          *    to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki.
00045          *
00046          *  <div align=center>
00047          *   <img src="CPose2D.gif">
00048          *  </div>
00049          *
00050          * \sa CPoseOrPoint,CPoint2D
00051          */
00052         class BASE_IMPEXP CPose2D : public CPose<CPose2D>, public mrpt::utils::CSerializable
00053         {
00054         public:
00055                 // This must be added to any CSerializable derived class:
00056                 DEFINE_SERIALIZABLE( CPose2D )
00057 
00058         public:
00059                 mrpt::math::CArrayDouble<2>   m_coords; //!< [x,y]
00060 
00061         protected:
00062                 double m_phi;  //!< The orientation of the pose, in radians.
00063 
00064         public:
00065                  /** Default constructor (all coordinates to 0) */
00066                  CPose2D();
00067 
00068                  /** Constructor from an initial value of the pose.*/
00069                  CPose2D(const double x,const double y,const double phi);
00070 
00071                  /** Constructor from a CPoint2D object. */
00072                  CPose2D(const CPoint2D &);
00073 
00074                  /** Aproximation!! Avoid its use, since information is lost. */
00075                  explicit CPose2D(const CPose3D &);
00076 
00077                  /** Constructor from lightweight object. */
00078                  CPose2D(const mrpt::math::TPose2D &);
00079 
00080                  /** Constructor from CPoint3D with information loss. */
00081                  explicit CPose2D(const CPoint3D &);
00082 
00083                  /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
00084                  inline CPose2D(TConstructorFlags_Poses constructor_dummy_param) { }
00085 
00086                  /** Get the phi angle of the 2D pose (in radians) */
00087                  inline const double &phi() const { return m_phi; }
00088                  //! \overload
00089                  inline       double &phi()       { return m_phi; }
00090 
00091                  /** Set the phi angle of the 2D pose (in radians) */
00092                  inline void phi(double angle) { m_phi=angle; }
00093 
00094                  inline void phi_incr(const double Aphi) { m_phi+=Aphi; }  //!< Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed)
00095 
00096                 /** Returns a 1x3 vector with [x y phi] */
00097                 void getAsVector(vector_double &v) const;
00098 
00099                  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
00100                    * \sa getInverseHomogeneousMatrix
00101                    */
00102                  void  getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const;
00103 
00104                  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
00105                   */
00106                  CPose2D  operator + (const CPose2D& D) const ;
00107 
00108                  /** Makes \f$ this = A \oplus B \f$
00109                   *  \note A or B can be "this" without problems.
00110                   */
00111                  void composeFrom(const CPose2D &A, const CPose2D &B);
00112 
00113                  /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
00114                   */
00115                  CPose3D  operator + (const CPose3D& D) const ;
00116 
00117                  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator.
00118                    */
00119                  CPoint2D operator + (const CPoint2D& u) const ;
00120 
00121                  /** 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.  */
00122                  void composePoint(double lx,double ly,double &gx, double &gy) const;
00123 
00124                  /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator.
00125                    */
00126                  CPoint3D operator + (const CPoint3D& u) const ;
00127 
00128                 /**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
00129                   *  \note A or B can be "this" without problems.
00130                   * \sa composeFrom, composePoint
00131                   */
00132                 void inverseComposeFrom(const CPose2D& A, const CPose2D& B );
00133 
00134                 /** Compute \f$ RET = this \oplus b \f$  */
00135                 inline CPose2D  operator - (const CPose2D& b) const
00136                 {
00137                         CPose2D ret(UNINITIALIZED_POSE);
00138                         ret.inverseComposeFrom(*this,b);
00139                         return ret;
00140                 }
00141 
00142                  /** Scalar sum of components: This is diferent from poses
00143                   *    composition, which is implemented as "+" operators in "CPose" derived classes.
00144                   */
00145                  void AddComponents(CPose2D &p);
00146 
00147                  /** Scalar multiplication.
00148                   */
00149                  void operator *=(const double  s);
00150 
00151                  /** Make \f$ this = this \oplus b \f$  */
00152                  inline CPose2D&  operator += (const CPose2D& b)
00153                  {
00154                         composeFrom(*this,b);
00155                         return *this;
00156                  }
00157 
00158                  /** Forces "phi" to be in the range [-pi,pi];
00159                    */
00160                  void  normalizePhi();
00161 
00162                  /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
00163                    * \sa fromString
00164                    */
00165                 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x(),y(),RAD2DEG(m_phi)); }
00166                 inline std::string asString() const { std::string s; asString(s); return s; }
00167 
00168                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
00169                    * \sa asString
00170                    * \exception std::exception On invalid format
00171                    */
00172                  void fromString(const std::string &s) {
00173                         CMatrixDouble  m;
00174                         if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00175                         ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
00176                         x( m.get_unsafe(0,0) );
00177                         y( m.get_unsafe(0,1) );
00178                         phi( DEG2RAD(m.get_unsafe(0,2)) );
00179                  }
00180 
00181                  inline const double &operator[](unsigned int i)const
00182                  {
00183                         switch(i)
00184                         {
00185                                 case 0:return m_coords[0];
00186                                 case 1:return m_coords[1];
00187                                 case 2:return m_phi;
00188                                 default:
00189                                 throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
00190                         }
00191                  }
00192                  inline double &operator[](unsigned int i)
00193                  {
00194                         switch(i)
00195                         {
00196                                 case 0:return m_coords[0];
00197                                 case 1:return m_coords[1];
00198                                 case 2:return m_phi;
00199                                 default:
00200                                 throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
00201                         }
00202                  }
00203 
00204                 /** makes: this = p (+) this */
00205                 inline void  changeCoordinatesReference( const CPose2D & p ) { composeFrom(p,CPose2D(*this)); }
00206 
00207                 typedef CPose2D  type_value; //!< Used to emulate CPosePDF types, for example, in CNetworkOfPoses
00208                 enum { is_3D_val = 0 };
00209                 static inline bool is_3D() { return is_3D_val!=0; }
00210                 enum { rotation_dimensions = 2 };
00211                 enum { is_PDF_val = 0 };
00212                 static inline bool is_PDF() { return is_PDF_val!=0; }
00213 
00214                 /** @name STL-like methods and typedefs
00215                    @{   */
00216                 typedef double         value_type;              //!< The type of the elements
00217                 typedef double&        reference;
00218                 typedef const double&  const_reference;
00219                 typedef std::size_t    size_type;
00220                 typedef std::ptrdiff_t difference_type;
00221 
00222                  // size is constant
00223                 enum { static_size = 3 };
00224                 static inline size_type size() { return static_size; }
00225                 static inline bool empty() { return false; }
00226                 static inline size_type max_size() { return static_size; }
00227                 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))); }
00228 
00229                 /** @} */
00230 
00231         }; // End of class def.
00232 
00233 
00234         std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose2D& p);
00235 
00236         /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y phi) */
00237         CPose2D BASE_IMPEXP operator -(const CPose2D &p);
00238 
00239         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.
00240 
00241         bool BASE_IMPEXP operator==(const CPose2D &p1,const CPose2D &p2);
00242         bool BASE_IMPEXP operator!=(const CPose2D &p1,const CPose2D &p2);
00243 
00244         typedef mrpt::aligned_containers<CPose2D>::vector_t             StdVector_CPose2D; //!< Eigen aligment-compatible container
00245         typedef mrpt::aligned_containers<CPose2D>::deque_t              StdDeque_CPose2D; //!< Eigen aligment-compatible container
00246 
00247         } // End of namespace
00248 } // End of namespace
00249 
00250 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011