00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00040 enum TConstructorFlags_Quaternions
00041 {
00042 UNINITIALIZED_QUATERNION = 0
00043 };
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 template <class T>
00060 class CQuaternion : public CArrayNumeric<T,4>
00061 {
00062 typedef CArrayNumeric<T,4> Base;
00063 public:
00064
00065
00066
00067
00068 inline CQuaternion(TConstructorFlags_Quaternions constructor_dummy_param) { }
00069
00070
00071 inline CQuaternion()
00072 {
00073 (*this)[0] = 1;
00074 (*this)[1] = 0;
00075 (*this)[2] = 0;
00076 (*this)[3] = 0;
00077 }
00078
00079
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];}
00094 inline T x()const {return (*this)[1];}
00095 inline T y()const {return (*this)[2];}
00096 inline T z()const {return (*this)[3];}
00097 inline void r(const T r) {(*this)[0]=r;}
00098 inline void x(const T x) {(*this)[1]=x;}
00099 inline void y(const T y) {(*this)[2]=y;}
00100 inline void z(const T z) {(*this)[3]=z;}
00101
00102
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
00132
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
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
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
00166 inline double normSqr() const { return mrpt::utils::square(r()) + mrpt::utils::square(x()) + mrpt::utils::square(y()) + mrpt::utils::square(z()); }
00167
00168
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
00178
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
00208
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
00221 template <class MATRIXLIKE>
00222 inline void rotationMatrix(MATRIXLIKE &M) const
00223 {
00224 M.setSize(3,3);
00225 rotationMatrixNoResize(M);
00226 }
00227
00228
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
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
00248 inline CQuaternion conj() const
00249 {
00250 CQuaternion q_aux;
00251 conj(q_aux);
00252 return q_aux;
00253 }
00254
00255
00256
00257
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
00265
00266
00267
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 {
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 {
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 {
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
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
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
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
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 };
00348
00349 typedef CQuaternion<double> CQuaternionDouble;
00350 typedef CQuaternion<float> CQuaternionFloat;
00351
00352 }
00353
00354 }
00355
00356 #endif