Main MRPT website > C++ reference
MRPT logo

CPose3DQuat.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 CPose3DQuat_H
00029 #define CPose3DQuat_H
00030 
00031 #include <mrpt/poses/CPose.h>
00032 #include <mrpt/math/CMatrixFixedNumeric.h>
00033 #include <mrpt/math/CQuaternion.h>
00034 #include <mrpt/poses/CPoint3D.h>
00035 #include <mrpt/math/lightweight_geom_data.h>
00036 
00037 namespace mrpt
00038 {
00039 namespace poses
00040 {
00041         using namespace mrpt::math;
00042 
00043         class CPose3D;
00044 
00045         DEFINE_SERIALIZABLE_PRE( CPose3DQuat )
00046 
00047         /** A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
00048          *
00049          *  For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
00050          *    to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki.
00051          *
00052          *  To access the translation use x(), y() and z(). To access the rotation, use CPose3DQuat::quat().
00053          *
00054          *  This class also behaves like a STL container, since it has begin(), end(), iterators, and can be accessed with the [] operator
00055          *   with indices running from 0 to 6 to access the  [x y z qr qx qy qz] as if they were a vector. Thus, a CPose3DQuat can be used
00056          *   as a 7-vector anywhere the MRPT math functions expect any kind of vector.
00057          *
00058          *  This class and CPose3D are very similar, and they can be converted to the each other automatically via transformation constructors.
00059          *
00060          * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion), mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic version of this class,  mrpt::math::CQuaternion, CPoseOrPoint
00061          */
00062         class BASE_IMPEXP CPose3DQuat : public CPose<CPose3DQuat>, public mrpt::utils::CSerializable
00063         {
00064                 // This must be added to any CSerializable derived class:
00065                 DEFINE_SERIALIZABLE( CPose3DQuat )
00066 
00067         public:
00068                 CArrayDouble<3>                 m_coords; //!< The translation vector [x,y,z]
00069                 mrpt::math::CQuaternionDouble   m_quat;   //!< The quaternion.
00070 
00071         public:
00072                 /** Read/Write access to the quaternion representing the 3D rotation. */
00073                 inline       mrpt::math::CQuaternionDouble & quat()       { return m_quat; }
00074                 /** Read-only access to the quaternion representing the 3D rotation. */
00075                 inline const mrpt::math::CQuaternionDouble & quat() const { return m_quat; }
00076 
00077                 /** Read/Write access to the translation vector in R^3. */
00078                 inline       mrpt::math::CArrayDouble<3> & xyz()       { return m_coords; }
00079                 /** Read-only access to the translation vector in R^3. */
00080                 inline const mrpt::math::CArrayDouble<3> & xyz() const { return m_coords; }
00081 
00082 
00083                 /** Default constructor, initialize translation to zeros and quaternion to no rotation. */
00084                 inline CPose3DQuat() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; }
00085 
00086                 /** Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use UNINITIALIZED_POSE as argument to this constructor. */
00087                 inline CPose3DQuat(TConstructorFlags_Quaternions constructor_dummy_param) : m_quat(UNINITIALIZED_QUATERNION) { }
00088 
00089                 /** Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary */
00090                 inline CPose3DQuat(const double x,const double y,const double z,const mrpt::math::CQuaternionDouble &q ) : m_quat(q) { m_coords[0]=x; m_coords[1]=y; m_coords[2]=z; m_quat.normalize(); }
00091 
00092                 /** Constructor from a CPose3D */
00093                 CPose3DQuat(const CPose3D &p);
00094 
00095                 /** Constructor from lightweight object. */
00096                 CPose3DQuat(const mrpt::math::TPose3DQuat &p) : m_quat(p.qr,p.qx,p.qy,p.qz) { x()=p.x; y()=p.y; z()=p.z; }
00097 
00098                 /** Constructor from a 4x4 homogeneous transformation matrix.
00099                   */
00100                 explicit CPose3DQuat(const CMatrixDouble44 &M);
00101 
00102                 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
00103                   * \sa getInverseHomogeneousMatrix
00104                   */
00105                 void  getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const;
00106 
00107                 /** Returns a 1x7 vector with [x y z qr qx qy qz] */
00108                 void getAsVector(vector_double &v) const;
00109 
00110                 /**  Makes \f$ this = A \oplus B \f$  this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
00111                   *  \note A or B can be "this" without problems.
00112                   * \sa inverseComposeFrom, composePoint
00113                   */
00114                 void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
00115 
00116                 /**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
00117                   *  \note A or B can be "this" without problems.
00118                   * \sa composeFrom, composePoint
00119                   */
00120                 void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
00121 
00122                 /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
00123                   * \sa composeFrom, inverseComposePoint
00124                   */
00125                 void composePoint(const double lx,const double ly,const double lz,double &gx,double &gy,double &gz,
00126                          mrpt::math::CMatrixFixedNumeric<double,3,3>  *out_jacobian_df_dpoint = NULL,
00127                          mrpt::math::CMatrixFixedNumeric<double,3,7>  *out_jacobian_df_dpose = NULL ) const;
00128 
00129                 /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
00130                   * \sa composePoint, composeFrom
00131                   */
00132                 void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
00133                          mrpt::math::CMatrixFixedNumeric<double,3,3>  *out_jacobian_df_dpoint = NULL,
00134                          mrpt::math::CMatrixFixedNumeric<double,3,7>  *out_jacobian_df_dpose = NULL ) const;
00135 
00136                 /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
00137                   *  POINT1 and POINT1 can be anything supporing [0],[1],[2].
00138                   * \sa composePoint    */
00139                 template <class POINT1,class POINT2> inline void composePoint( const POINT1 &L, POINT2 &G) const { composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); }
00140 
00141                 /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.  \sa inverseComposePoint */
00142                 template <class POINT1,class POINT2> inline void inverseComposePoint( const POINT1 &G, POINT2 &L) const { inverseComposePoint(G[0],G[1],G[2],L[0],L[1],L[2]); }
00143 
00144                 /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.  \sa composePoint    */
00145                 inline CPoint3D operator +( const CPoint3D &L) const { CPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
00146 
00147                 /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.  \sa composePoint    */
00148                 inline TPoint3D operator +( const TPoint3D &L) const { TPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
00149 
00150                 /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.  \sa inverseComposePoint    */
00151                 inline CPoint3D operator -( const CPoint3D &G) const { CPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
00152 
00153                 /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.  \sa inverseComposePoint    */
00154                 inline TPoint3D operator -( const TPoint3D &G) const { TPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
00155 
00156                 /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
00157                   */
00158                 virtual void operator *=(const double  s);
00159 
00160                 /** Make \f$ this = this \oplus b \f$  */
00161                 inline CPose3DQuat&  operator += (const CPose3DQuat& b)
00162                 {
00163                         composeFrom(*this,b);
00164                         return *this;
00165                 }
00166 
00167                 /** Return the composed pose \f$ ret = this \oplus p \f$  */
00168                 inline CPose3DQuat operator + (const CPose3DQuat& p) const
00169                 {
00170                         CPose3DQuat ret;
00171                         ret.composeFrom(*this,p);
00172                         return ret;
00173                 }
00174 
00175                 /** Make \f$ this = this \ominus b \f$  */
00176                 inline CPose3DQuat&  operator -= (const CPose3DQuat& b)
00177                 {
00178                         inverseComposeFrom(*this,b);
00179                         return *this;
00180                 }
00181 
00182                 /** Return the composed pose \f$ ret = this \ominus p \f$  */
00183                 inline CPose3DQuat operator - (const CPose3DQuat& p) const
00184                 {
00185                         CPose3DQuat ret;
00186                         ret.inverseComposeFrom(*this,p);
00187                         return ret;
00188                 }
00189 
00190                  /** Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]", angles in degrees.)
00191                    * \sa fromString
00192                    */
00193                 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],m_quat[0],m_quat[1],m_quat[2],m_quat[3]); }
00194                 inline std::string asString() const { std::string s; asString(s); return s; }
00195 
00196                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1 0 0 0]" )
00197                    * \sa asString
00198                    * \exception std::exception On invalid format
00199                    */
00200                  void fromString(const std::string &s) {
00201                         CMatrixDouble  m;
00202                         if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00203                         ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
00204                         m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2);
00205                         m_quat[0] = m.get_unsafe(0,3); m_quat[1] = m.get_unsafe(0,4); m_quat[2] = m.get_unsafe(0,5); m_quat[3] = m.get_unsafe(0,6);
00206                  }
00207 
00208                 /** Read only [] operator */
00209                 inline const double &operator[](unsigned int i) const
00210                 {
00211                         switch(i)
00212                         {
00213                                 case 0:return m_coords[0];
00214                                 case 1:return m_coords[1];
00215                                 case 2:return m_coords[2];
00216                                 case 3:return m_quat[0];
00217                                 case 4:return m_quat[1];
00218                                 case 5:return m_quat[2];
00219                                 case 6:return m_quat[3];
00220                                 default:
00221                                 throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
00222                         }
00223                 }
00224                 /** Read/write [] operator */
00225                 inline double &operator[](unsigned int i)
00226                 {
00227                         switch(i)
00228                         {
00229                                 case 0:return m_coords[0];
00230                                 case 1:return m_coords[1];
00231                                 case 2:return m_coords[2];
00232                                 case 3:return m_quat[0];
00233                                 case 4:return m_quat[1];
00234                                 case 5:return m_quat[2];
00235                                 case 6:return m_quat[3];
00236                                 default:
00237                                 throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
00238                         }
00239                 }
00240 
00241         /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
00242           *  For the coordinate system see the top of this page.
00243                   *  If the matrix pointers are not NULL, the Jacobians will be also computed for the range-yaw-pitch variables wrt the passed 3D point and this 7D pose.
00244           */
00245         void sphericalCoordinates(
00246             const TPoint3D &point,
00247             double &out_range,
00248             double &out_yaw,
00249             double &out_pitch,
00250                         mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint = NULL,
00251                         mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose = NULL
00252                         ) const;
00253 
00254         public:
00255                 typedef CPose3DQuat type_value; //!< Used to emulate CPosePDF types, for example, in CNetworkOfPoses
00256                 enum { is_3D_val = 1 };
00257                 static inline bool is_3D() { return is_3D_val!=0; }
00258                 enum { rotation_dimensions = 3 };
00259                 enum { is_PDF_val = 1 };
00260                 static inline bool is_PDF() { return is_PDF_val!=0; }
00261 
00262 
00263                 /** @name STL-like methods and typedefs
00264                    @{   */
00265                 typedef double         value_type;              //!< The type of the elements
00266                 typedef double&        reference;
00267                 typedef const double&  const_reference;
00268                 typedef std::size_t    size_type;
00269                 typedef std::ptrdiff_t difference_type;
00270 
00271                 // size is constant
00272                 enum { static_size = 7 };
00273                 static inline size_type size() { return static_size; }
00274                 static inline bool empty() { return false; }
00275                 static inline size_type max_size() { return static_size; }
00276                 static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3DQuat to %u.",static_cast<unsigned>(n))); }
00277 
00278                 inline void assign(const size_t N, const double val)
00279                 {
00280                         if (N!=7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
00281                         m_coords.fill(val);
00282                         m_quat.fill(val);
00283                 }
00284 
00285                 struct iterator : public std::iterator<std::random_access_iterator_tag,value_type>
00286                 {
00287                 private:
00288                         typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
00289                         CPose3DQuat *m_obj;             //!< A reference to the source of this iterator
00290                         size_t          m_cur_idx;      //!< The iterator points to this element.
00291                         typedef value_type T; //!< The type of the matrix elements
00292 
00293                         inline void check_limits(bool allow_end = false) const
00294                         {
00295         #ifdef _DEBUG
00296                                 ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
00297                                 if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
00298         #endif
00299                         }
00300                 public:
00301                         inline bool operator <(const iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
00302                         inline bool operator >(const iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
00303                         inline iterator() : m_obj(NULL),m_cur_idx(0) { }
00304                         inline iterator(CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) {  check_limits(true); /*Dont report as error an iterator to end()*/ }
00305                         inline CPose3DQuat::reference operator*() const {  check_limits();  return (*m_obj)[m_cur_idx];  }
00306                         inline iterator &operator++() {
00307                                 check_limits();
00308                                 ++m_cur_idx;
00309                                 return *this;
00310                         }
00311                         inline iterator operator++(int) {
00312                                 iterator it=*this;
00313                                 ++*this;
00314                                 return it;
00315                         }
00316                         inline iterator &operator--()   {
00317                                 --m_cur_idx;
00318                                 check_limits();
00319                                 return *this;
00320                         }
00321                         inline iterator operator--(int) {
00322                                 iterator it=*this;
00323                                 --*this;
00324                                 return it;
00325                         }
00326                         inline iterator &operator+=(iterator_base::difference_type off) {
00327                                 m_cur_idx+=off;
00328                                 check_limits(true);
00329                                 return *this;
00330                         }
00331                         inline iterator operator+(iterator_base::difference_type off) const     {
00332                                 iterator it=*this;
00333                                 it+=off;
00334                                 return it;
00335                         }
00336                         inline iterator &operator-=(iterator_base::difference_type off) {
00337                                 return (*this)+=(-off);
00338                         }
00339                         inline iterator operator-(iterator_base::difference_type off) const     {
00340                                 iterator it=*this;
00341                                 it-=off;
00342                                 return it;
00343                         }
00344                         inline iterator_base::difference_type operator-(const iterator &it) const       { return m_cur_idx - it.m_cur_idx; }
00345                         inline CPose3DQuat::reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
00346                         inline bool operator==(const iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
00347                         inline bool operator!=(const iterator &it) const { return !(operator==(it)); }
00348                 }; // end iterator
00349 
00350                 struct const_iterator : public std::iterator<std::random_access_iterator_tag,value_type>
00351                 {
00352                 private:
00353                         typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
00354                         const CPose3DQuat *m_obj;               //!< A reference to the source of this iterator
00355                         size_t          m_cur_idx;      //!< The iterator points to this element.
00356                         typedef value_type T; //!< The type of the matrix elements
00357 
00358                         inline void check_limits(bool allow_end = false) const
00359                         {
00360         #ifdef _DEBUG
00361                                 ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
00362                                 if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
00363         #endif
00364                         }
00365                 public:
00366                         inline bool operator <(const const_iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
00367                         inline bool operator >(const const_iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
00368                         inline const_iterator() : m_obj(NULL),m_cur_idx(0) { }
00369                         inline const_iterator(const CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) {  check_limits(true); /*Dont report as error an iterator to end()*/ }
00370                         inline CPose3DQuat::const_reference operator*() const   {  check_limits();  return (*m_obj)[m_cur_idx];  }
00371                         inline const_iterator &operator++() {
00372                                 check_limits();
00373                                 ++m_cur_idx;
00374                                 return *this;
00375                         }
00376                         inline const_iterator operator++(int)   {
00377                                 const_iterator it=*this;
00378                                 ++*this;
00379                                 return it;
00380                         }
00381                         inline const_iterator &operator--()     {
00382                                 --m_cur_idx;
00383                                 check_limits();
00384                                 return *this;
00385                         }
00386                         inline const_iterator operator--(int)   {
00387                                 const_iterator it=*this;
00388                                 --*this;
00389                                 return it;
00390                         }
00391                         inline const_iterator &operator+=(iterator_base::difference_type off)   {
00392                                 m_cur_idx+=off;
00393                                 check_limits(true);
00394                                 return *this;
00395                         }
00396                         inline const_iterator operator+(iterator_base::difference_type off) const       {
00397                                 const_iterator it=*this;
00398                                 it+=off;
00399                                 return it;
00400                         }
00401                         inline const_iterator &operator-=(iterator_base::difference_type off)   {
00402                                 return (*this)+=(-off);
00403                         }
00404                         inline const_iterator operator-(iterator_base::difference_type off) const       {
00405                                 const_iterator it=*this;
00406                                 it-=off;
00407                                 return it;
00408                         }
00409                         inline iterator_base::difference_type operator-(const const_iterator &it) const { return m_cur_idx - it.m_cur_idx; }
00410                         inline CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
00411                         inline bool operator==(const const_iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
00412                         inline bool operator!=(const const_iterator &it) const { return !(operator==(it)); }
00413                 }; // end const_iterator
00414 
00415                 typedef std::reverse_iterator<iterator>                 reverse_iterator;
00416                 typedef std::reverse_iterator<const_iterator>   const_reverse_iterator;
00417                 inline iterator                 begin()   { return iterator(*this,0); }
00418                 inline iterator                 end()     { return iterator(*this,static_size); }
00419                 inline const_iterator   begin() const   { return const_iterator(*this,0); }
00420                 inline const_iterator   end() const             { return const_iterator(*this,static_size); }
00421                 inline reverse_iterator                 rbegin()                { return reverse_iterator(end()); }
00422                 inline const_reverse_iterator   rbegin() const  { return const_reverse_iterator(end()); }
00423                 inline reverse_iterator                 rend()                  { return reverse_iterator(begin()); }
00424                 inline const_reverse_iterator   rend() const    { return const_reverse_iterator(begin()); }
00425 
00426 
00427                 void swap (CPose3DQuat& o)
00428                 {
00429                         std::swap(o.m_coords, m_coords);
00430                         o.m_quat.swap(m_quat);
00431                 }
00432 
00433                 /** @} */
00434                 //! See ops_containers.h
00435                 typedef CPose3DQuat  mrpt_autotype;
00436                 //DECLARE_MRPT_CONTAINER_TYPES
00437 
00438         }; // End of class def.
00439 
00440         std::ostream BASE_IMPEXP  & operator << (std::ostream& o, const CPose3DQuat& p);
00441 
00442 
00443         } // End of namespace
00444 } // End of namespace
00445 
00446 #endif



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