Main MRPT website > C++ reference
MRPT logo

CQuaternion.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 
00029 #ifndef CQuaternion_H
00030 #define CQuaternion_H
00031 
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/math/CArray.h>
00034 
00035 namespace mrpt
00036 {
00037         namespace math
00038         {
00039                 // For use with a constructor of CQuaternion
00040                 enum TConstructorFlags_Quaternions
00041                 {
00042                         UNINITIALIZED_QUATERNION = 0
00043                 };
00044 
00045                 /** A quaternion, which can represent a 3D rotation as pair \f$ (r,\mathbf{u}) \f$, with a real part "r" and a 3D vector \f$ \mathbf{u} = (x,y,z) \f$, or alternatively, q = r + ix + jy + kz.
00046                  *
00047                  *  The elements of the quaternion can be accessed by either:
00048                  *              - r(), x(), y(), z(), or
00049                  *              - the operator [] with indices running from 0 (=r) to 3 (=z).
00050                  *
00051                  *  Users will usually employ the typedef "CQuaternionDouble" instead of this template.
00052                  *
00053                  * For more information about quaternions, see:
00054                  *  - http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
00055                  *  - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
00056                  *
00057                  * \sa mrpt::poses::CPose3D
00058                  */
00059                 template <class T>
00060                 class CQuaternion : public CArrayNumeric<T,4>
00061                 {
00062                         typedef CArrayNumeric<T,4> Base;
00063                 public:
00064         /* @{ Constructors
00065          */
00066 
00067                 /**     Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quaternion (use this constructor when speed is critical). */
00068                 inline CQuaternion(TConstructorFlags_Quaternions constructor_dummy_param) { }
00069 
00070                 /**     Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation. */
00071                 inline CQuaternion()
00072                 {
00073                         (*this)[0] = 1;
00074                         (*this)[1] = 0;
00075                         (*this)[2] = 0;
00076                         (*this)[3] = 0;
00077                 }
00078 
00079                 /**     Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz. */
00080                 inline CQuaternion(const T r,const T x,const T y,const T z)
00081                 {
00082                         (*this)[0] = r;
00083                         (*this)[1] = x;
00084                         (*this)[2] = y;
00085                         (*this)[3] = z;
00086                         ASSERTDEBMSG_(std::abs(normSqr()-1.0)<1e-5, mrpt::format("Initialization data for quaternion is not normalized: %f %f %f %f -> sqrNorm=%f",r,x,y,z,normSqr()) );
00087                 }
00088 
00089                 /* @}
00090                  */
00091 
00092 
00093                 inline T  r()const {return (*this)[0];} //!< Return r coordinate of the quaternion
00094                 inline T  x()const {return (*this)[1];} //!< Return x coordinate of the quaternion
00095                 inline T  y()const {return (*this)[2];} //!< Return y coordinate of the quaternion
00096                 inline T  z()const {return (*this)[3];} //!< Return z coordinate of the quaternion
00097                 inline void  r(const T r) {(*this)[0]=r;}       //!< Set r coordinate of the quaternion
00098                 inline void  x(const T x) {(*this)[1]=x;}       //!< Set x coordinate of the quaternion
00099                 inline void  y(const T y) {(*this)[2]=y;}       //!< Set y coordinate of the quaternion
00100                 inline void  z(const T z) {(*this)[3]=z;}       //!< Set z coordinate of the quaternion
00101 
00102                 /**     Set this quaternion to the rotation described by a 3D Rodrigues rotation vector.
00103                   */
00104                 template <class ARRAYLIKE>
00105                 void  fromRodriguesVector(const ARRAYLIKE &in)
00106                 {
00107                         if (in.size()!=3) THROW_EXCEPTION("Wrong Dimension in input vector for quaternion Constructor");
00108 
00109                         const T x = in[0];
00110                         const T y = in[1];
00111                         const T z = in[2];
00112                         if ((x==0)&&(y==0)&&(z==0))
00113                         {
00114                                 (*this)[0] = 1;
00115                                 (*this)[1] = 0;
00116                                 (*this)[2] = 0;
00117                                 (*this)[3] = 0;
00118                         }
00119                         else
00120                         {
00121                                 const T angle = sqrt(x*x+y*y+z*z);
00122                                 const T s = (sin(angle/2))/angle;
00123                                 const T c = cos(angle/2);
00124                                 (*this)[0] = c;
00125                                 (*this)[1] = x * s;
00126                                 (*this)[2] = y * s;
00127                                 (*this)[3] = z * s;
00128                         }
00129                 }
00130 
00131                 /**     Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2
00132                   *   After the operation, "this" will represent the composed rotations of q1 and q2 (q2 applied "after" q1).
00133                   */
00134                 inline void  crossProduct(const CQuaternion &q1, const CQuaternion &q2)
00135                 {
00136                         this->r(  q1.r()*q2.r() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z() );
00137                         this->x(  q1.r()*q2.x() + q2.r()*q1.x() + q1.y()*q2.z() - q2.y()*q1.z() );
00138             this->y(  q1.r()*q2.y() + q2.r()*q1.y() + q1.z()*q2.x() - q2.z()*q1.x() );
00139                         this->z(  q1.r()*q2.z() + q2.r()*q1.z() + q1.x()*q2.y() - q2.x()*q1.y() );
00140                         this->normalize();
00141                 }
00142 
00143                 /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion
00144                   */
00145                 void rotatePoint(const double lx,const double ly,const double lz, double &gx,double &gy,double &gz ) const
00146                 {
00147                         const double t2 = r()*x(); const double t3 = r()*y(); const double t4 = r()*z(); const double t5 =-x()*x(); const double t6 = x()*y();
00148                         const double t7 = x()*z(); const double t8 =-y()*y(); const double t9 = y()*z(); const double t10=-z()*z();
00149                         gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
00150                         gy = 2*((t4+  t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
00151                         gz = 2*((t7-  t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
00152                 }
00153 
00154                 /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion
00155                   */
00156                 void inverseRotatePoint(const double lx,const double ly,const double lz, double &gx,double &gy,double &gz ) const
00157                 {
00158                         const double t2 =-r()*x(); const double t3 =-r()*y(); const double t4 =-r()*z(); const double t5 =-x()*x(); const double t6 = x()*y();
00159                         const double t7 = x()*z(); const double t8 =-y()*y(); const double t9 = y()*z(); const double t10=-z()*z();
00160                         gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
00161                         gy = 2*((t4+  t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
00162                         gz = 2*((t7-  t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
00163                 }
00164 
00165                 /** Return the squared norm of the quaternion */
00166                 inline double normSqr() const { return mrpt::utils::square(r()) + mrpt::utils::square(x()) + mrpt::utils::square(y()) + mrpt::utils::square(z()); }
00167 
00168                 /**     Normalize this quaternion, so its norm becomes the unitity.
00169                   */
00170                 inline void normalize()
00171                 {
00172                         const T qq = 1.0/std::sqrt( normSqr() );
00173                         for (unsigned int i=0;i<4;i++)
00174                                 (*this)[i] *= qq;
00175                 }
00176 
00177                 /** Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
00178                   *  The output matrix can be a dynamic or fixed size (4x4) matrix.
00179                   */
00180                 template <class MATRIXLIKE>
00181                 void  normalizationJacobian(MATRIXLIKE &J) const
00182                 {
00183                         const T n = 1.0/std::pow(normSqr(),T(1.5));
00184                         J.setSize(4,4);
00185                         J.get_unsafe(0,0)=x()*x()+y()*y()+z()*z();
00186                         J.get_unsafe(0,1)=-r()*x();
00187                         J.get_unsafe(0,2)=-r()*y();
00188                         J.get_unsafe(0,3)=-r()*z();
00189 
00190                         J.get_unsafe(1,0)=-x()*r();
00191                         J.get_unsafe(1,1)=r()*r()+y()*y()+z()*z();
00192                         J.get_unsafe(1,2)=-x()*y();
00193                         J.get_unsafe(1,3)=-x()*z();
00194 
00195                         J.get_unsafe(2,0)=-y()*r();
00196                         J.get_unsafe(2,1)=-y()*x();
00197                         J.get_unsafe(2,2)=r()*r()+x()*x()+z()*z();
00198                         J.get_unsafe(2,3)=-y()*z();
00199 
00200                         J.get_unsafe(3,0)=-z()*r();
00201                         J.get_unsafe(3,1)=-z()*x();
00202                         J.get_unsafe(3,2)=-z()*y();
00203                         J.get_unsafe(3,3)=r()*r()+x()*x()+y()*y();
00204                         J *=n;
00205                 }
00206 
00207                 /** Compute the Jacobian of the rotation composition operation \f$ p = f(\cdot) = q_{this} \times r \f$, that is the 4x4 matrix \f$ \frac{\partial f}{\partial q_{this} }  \f$.
00208                   *  The output matrix can be a dynamic or fixed size (4x4) matrix.
00209                   */
00210                 template <class MATRIXLIKE>
00211                 inline void rotationJacobian(MATRIXLIKE &J) const
00212                 {
00213                         J.setSize(4,4);
00214                         J.get_unsafe(0,0)=r(); J.get_unsafe(0,1)=-x(); J.get_unsafe(0,2)=-y(); J.get_unsafe(0,3)=-z();
00215                         J.get_unsafe(1,0)=x(); J.get_unsafe(1,1)= r(); J.get_unsafe(1,2)=-z(); J.get_unsafe(1,3)= y();
00216                         J.get_unsafe(2,0)=y(); J.get_unsafe(2,1)= z(); J.get_unsafe(2,2)= r(); J.get_unsafe(2,3)=-x();
00217                         J.get_unsafe(3,0)=z(); J.get_unsafe(3,1)=-y(); J.get_unsafe(3,2)= x(); J.get_unsafe(3,3)= r();
00218                 }
00219 
00220                 /** Calculate the 3x3 rotation matrix associated to this quaternion */
00221                 template <class MATRIXLIKE>
00222         inline void  rotationMatrix(MATRIXLIKE &M) const
00223                 {
00224                         M.setSize(3,3);
00225                         rotationMatrixNoResize(M);
00226                 }
00227 
00228                 /** Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quaternion (does not resize the matrix, for that, see rotationMatrix) */
00229                 template <class MATRIXLIKE>
00230         inline void  rotationMatrixNoResize(MATRIXLIKE &M) const
00231                 {
00232                         M.get_unsafe(0,0)=r()*r()+x()*x()-y()*y()-z()*z();              M.get_unsafe(0,1)=2*(x()*y() -r()*z());                 M.get_unsafe(0,2)=2*(z()*x()+r()*y());
00233                         M.get_unsafe(1,0)=2*(x()*y()+r()*z());                          M.get_unsafe(1,1)=r()*r()-x()*x()+y()*y()-z()*z();              M.get_unsafe(1,2)=2*(y()*z()-r()*x());
00234                         M.get_unsafe(2,0)=2*(z()*x()-r()*y());                          M.get_unsafe(2,1)=2*(y()*z()+r()*x());                          M.get_unsafe(2,2)=r()*r()-x()*x()-y()*y()+z()*z();
00235                 }
00236 
00237 
00238                 /**     Return the conjugate quaternion  */
00239                 inline void conj(CQuaternion &q_out) const
00240                 {
00241                         q_out.r( r() );
00242                         q_out.x(-x() );
00243                         q_out.y(-y() );
00244                         q_out.z(-z() );
00245                 }
00246 
00247                 /**     Return the conjugate quaternion  */
00248                 inline CQuaternion conj() const
00249                 {
00250                         CQuaternion q_aux;
00251                         conj(q_aux);
00252                         return q_aux;
00253                 }
00254 
00255                 /**     Return the yaw, pitch & roll angles associated to quaternion
00256                   *  \sa For the equations, see The MRPT Book, or see http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
00257                   *  \sa rpy_and_jacobian
00258                 */
00259                 inline void rpy(T &roll, T &pitch, T &yaw) const
00260                 {
00261                         rpy_and_jacobian(roll,pitch,yaw,static_cast<mrpt::math::CMatrixDouble*>(NULL));
00262                 }
00263 
00264                 /**     Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of the transformation.
00265                   *  Note that both the angles and the Jacobian have one set of normal equations, plus other special formulas for the degenerated cases of |pitch|=90 degrees.
00266                   *  \sa For the equations, see The MRPT Book, or http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
00267                   * \sa rpy
00268                 */
00269                 template <class MATRIXLIKE>
00270                 void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq = NULL, bool resize_out_dr_dq_to3x4 = true ) const
00271                 {
00272                         using mrpt::utils::square;
00273                         using std::sqrt;
00274 
00275                         if (out_dr_dq && resize_out_dr_dq_to3x4)
00276                                 out_dr_dq->setSize(3,4);
00277                         const T discr = r()*y()-x()*z();
00278                         if (fabs(discr)>0.49999)
00279                         {       // pitch = 90 deg
00280                                 pitch =  0.5*M_PI;
00281                                 yaw   =-2*atan2(x(),r());
00282                                 roll  = 0;
00283                                 if (out_dr_dq) {
00284                                         out_dr_dq->zeros();
00285                                         out_dr_dq->get_unsafe(0,0) = +2/x();
00286                                         out_dr_dq->get_unsafe(0,2) = -2*r()/(x()*x());
00287                                 }
00288                         }
00289                         else if (discr<-0.49999)
00290                         {       // pitch =-90 deg
00291                                 pitch = -0.5*M_PI;
00292                                 yaw   =+2*atan2(x(),r());
00293                                 roll  = 0;
00294                                 if (out_dr_dq) {
00295                                         out_dr_dq->zeros();
00296                                         out_dr_dq->get_unsafe(0,0) = -2/x();
00297                                         out_dr_dq->get_unsafe(0,2) = +2*r()/(x()*x());
00298                                 }
00299                         }
00300                         else
00301                         {       // Non-degenerate case:
00302                                 yaw   = atan2( 2*(r()*z()+x()*y()), 1-2*(y()*y()+z()*z()) );
00303                                 pitch = asin ( 2*discr );
00304                                 roll  = atan2( 2*(r()*x()+y()*z()), 1-2*(x()*x()+y()*y()) );
00305                                 if (out_dr_dq) {
00306                                         // Auxiliary terms:
00307                                         const double val1=(2*x()*x() + 2*y()*y() - 1);
00308                                         const double val12=square(val1);
00309                                         const double val2=(2*r()*x() + 2*y()*z());
00310                                         const double val22=square(val2);
00311                                         const double xy2 = 2*x()*y();
00312                                         const double rz2 = 2*r()*z();
00313                                         const double ry2 = 2*r()*y();
00314                                         const double val3 = (2*y()*y() + 2*z()*z() - 1);
00315                                         const double val4 = ((square(rz2 + xy2)/square(val3) + 1)*val3);
00316                                         const double val5 = (4*(rz2 + xy2))/square(val3);
00317                                         const double val6 = 1.0/(square(rz2 + xy2)/square(val3) + 1);
00318                                         const double val7 = 2.0/ sqrt(1 - square(ry2 - 2*x()*z()));
00319                                         const double val8 = (val22/val12 + 1);
00320                                         const double val9 = -2.0/val8;
00321                                         // row 1:
00322                                         out_dr_dq->get_unsafe(0,0) = -2*z()/val4;
00323                                         out_dr_dq->get_unsafe(0,1) = -2*y()/val4;
00324                                         out_dr_dq->get_unsafe(0,2) = -(2*x()/val3 - y()*val5)*val6 ;
00325                                         out_dr_dq->get_unsafe(0,3) = -(2*r()/val3 - z()*val5)*val6;
00326                                         // row 2:
00327                                         out_dr_dq->get_unsafe(1,0) = y()*val7  ;
00328                                         out_dr_dq->get_unsafe(1,1) = -z()*val7 ;
00329                                         out_dr_dq->get_unsafe(1,2) = r()*val7 ;
00330                                         out_dr_dq->get_unsafe(1,3) = -x()*val7 ;
00331                                         // row 3:
00332                                         out_dr_dq->get_unsafe(2,0) = val9*x()/val1 ;
00333                                         out_dr_dq->get_unsafe(2,1) = val9*(r()/val1 - (2*x()*val2)/val12) ;
00334                                         out_dr_dq->get_unsafe(2,2) = val9*(z()/val1 - (2*y()*val2)/val12) ;
00335                                         out_dr_dq->get_unsafe(2,3) = val9*y()/val1 ;
00336                                 }
00337                         }
00338                 }
00339 
00340                 inline CQuaternion  operator * (const T &factor)
00341                 {
00342                         CQuaternion q = *this;
00343                         q*=factor;
00344                         return q;
00345                 }
00346 
00347                 };      // end class
00348 
00349                 typedef CQuaternion<double> CQuaternionDouble;  //!< A quaternion of data type "double"
00350                 typedef CQuaternion<float>  CQuaternionFloat;   //!< A quaternion of data type "float"
00351 
00352         }       // end namespace
00353 
00354 } // end namespace mrpt
00355 
00356 #endif



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