Main MRPT website > C++ reference
MRPT logo
CQuaternion.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 
29 #ifndef CQuaternion_H
30 #define CQuaternion_H
31 
32 #include <mrpt/math/CMatrixTemplateNumeric.h>
33 #include <mrpt/math/CArray.h>
34 
35 namespace mrpt
36 {
37  namespace math
38  {
39  // For use with a constructor of CQuaternion
40  enum TConstructorFlags_Quaternions
41  {
42  UNINITIALIZED_QUATERNION = 0
43  };
44 
45  /** 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.
46  *
47  * The elements of the quaternion can be accessed by either:
48  * - r(), x(), y(), z(), or
49  * - the operator [] with indices running from 0 (=r) to 3 (=z).
50  *
51  * Users will usually employ the typedef "CQuaternionDouble" instead of this template.
52  *
53  * For more information about quaternions, see:
54  * - http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
55  * - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
56  *
57  * \ingroup mrpt_base_grp
58  * \sa mrpt::poses::CPose3D
59  */
60  template <class T>
61  class CQuaternion : public CArrayNumeric<T,4>
62  {
63  typedef CArrayNumeric<T,4> Base;
64  public:
65  /* @{ Constructors
66  */
67 
68  /** Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quaternion (use this constructor when speed is critical). */
69  inline CQuaternion(TConstructorFlags_Quaternions constructor_dummy_param) { }
70 
71  /** Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation. */
72  inline CQuaternion()
73  {
74  (*this)[0] = 1;
75  (*this)[1] = 0;
76  (*this)[2] = 0;
77  (*this)[3] = 0;
78  }
79 
80  /** Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz. */
81  inline CQuaternion(const T r,const T x,const T y,const T z)
82  {
83  (*this)[0] = r;
84  (*this)[1] = x;
85  (*this)[2] = y;
86  (*this)[3] = z;
87  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()) );
88  }
89 
90  /* @}
91  */
92 
93 
94  inline T r()const {return (*this)[0];} //!< Return r coordinate of the quaternion
95  inline T x()const {return (*this)[1];} //!< Return x coordinate of the quaternion
96  inline T y()const {return (*this)[2];} //!< Return y coordinate of the quaternion
97  inline T z()const {return (*this)[3];} //!< Return z coordinate of the quaternion
98  inline void r(const T r) {(*this)[0]=r;} //!< Set r coordinate of the quaternion
99  inline void x(const T x) {(*this)[1]=x;} //!< Set x coordinate of the quaternion
100  inline void y(const T y) {(*this)[2]=y;} //!< Set y coordinate of the quaternion
101  inline void z(const T z) {(*this)[3]=z;} //!< Set z coordinate of the quaternion
102 
103  /** Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector \f$ \mathbf{v} \f$:
104  * If \f$ \mathbf{v}=0 \f$, then the quaternion is \f$ \mathbf{q} = [1 ~ 0 ~ 0 ~ 0]^\top \f$, otherwise:
105  * \f[ \mathbf{q} = \left[ \begin{array}{c}
106  * \cos(\frac{\theta}{2}) \\
107  * v_x \frac{\sin(\frac{\theta}{2})}{\theta} \\
108  * v_y \frac{\sin(\frac{\theta}{2})}{\theta} \\
109  * v_z \frac{\sin(\frac{\theta}{2})}{\theta}
110  * \end{array} \right] \f]
111  * where \f$ \theta = |\mathbf{v}| = \sqrt{v_x^2+v_y^2+v_z^2} \f$.
112  * \sa "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)", James Diebel.
113  */
114  template <class ARRAYLIKE3>
115  void fromRodriguesVector(const ARRAYLIKE3 &v)
116  {
117  if (v.size()!=3) THROW_EXCEPTION("Vector v must have a length=3");
118 
119  const T x = v[0];
120  const T y = v[1];
121  const T z = v[2];
122  const T angle = std::sqrt(x*x+y*y+z*z);
123  if (angle<1e-7)
124  {
125  (*this)[0] = 1;
126  (*this)[1] = static_cast<T>(0.5)*x;
127  (*this)[2] = static_cast<T>(0.5)*y;
128  (*this)[3] = static_cast<T>(0.5)*z;
129  }
130  else
131  {
132  const T s = (::sin(angle/2))/angle;
133  const T c = ::cos(angle/2);
134  (*this)[0] = c;
135  (*this)[1] = x * s;
136  (*this)[2] = y * s;
137  (*this)[3] = z * s;
138  }
139  }
140 
141 
142  /** @name Lie Algebra methods
143  @{ */
144 
145 
146  /** Logarithm of the 3x3 matrix defined by this pose, generating the corresponding vector in the SO(3) Lie Algebra,
147  * which coincides with the so-called "rotation vector" (I don't have space here for the proof ;-).
148  * \param[out] out_ln The target vector, which can be: std::vector<>, or mrpt::vector_double or any row or column Eigen::Matrix<>.
149  * \sa exp, mrpt::poses::SE_traits */
150  template <class ARRAYLIKE3>
151  inline void ln(ARRAYLIKE3 &out_ln) const
152  {
153  if (out_ln.size()!=3) out_ln.resize(3);
154  this->ln_noresize(out_ln);
155  }
156  /** \overload that returns by value */
157  template <class ARRAYLIKE3>
158  inline ARRAYLIKE3 ln() const
159  {
160  ARRAYLIKE3 out_ln;
161  this->ln(out_ln);
162  return out_ln;
163  }
164  /** Like ln() but does not try to resize the output vector. */
165  template <class ARRAYLIKE3>
166  void ln_noresize(ARRAYLIKE3 &out_ln) const
167  {
168  using mrpt::utils::square;
169  const T xyz_norm = std::sqrt(square(x())+square(y())+square(z()));
170  const T K = (xyz_norm<1e-7) ? 2 : 2*::acos(r())/xyz_norm;
171  out_ln[0] = K*x();
172  out_ln[1] = K*y();
173  out_ln[2] = K*z();
174  }
175 
176  /** Exponential map from the SO(3) Lie Algebra to unit quaternions.
177  * \sa ln, mrpt::poses::SE_traits */
178  template <class ARRAYLIKE3>
179  inline static CQuaternion<T> exp(const ARRAYLIKE3 & v)
180  {
181  CQuaternion<T> q(UNINITIALIZED_QUATERNION);
182  q.fromRodriguesVector(v);
183  return q;
184  }
185  /** \overload */
186  template <class ARRAYLIKE3>
187  inline static void exp(const ARRAYLIKE3 & v, CQuaternion<T> & out_quat)
188  {
189  out_quat.fromRodriguesVector(v);
190  }
191 
192  /** @} */ // end of Lie algebra
193 
194 
195  /** Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2
196  * After the operation, "this" will represent the composed rotations of q1 and q2 (q2 applied "after" q1).
197  */
198  inline void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
199  {
200  this->r( q1.r()*q2.r() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z() );
201  this->x( q1.r()*q2.x() + q2.r()*q1.x() + q1.y()*q2.z() - q2.y()*q1.z() );
202  this->y( q1.r()*q2.y() + q2.r()*q1.y() + q1.z()*q2.x() - q2.z()*q1.x() );
203  this->z( q1.r()*q2.z() + q2.r()*q1.z() + q1.x()*q2.y() - q2.x()*q1.y() );
204  this->normalize();
205  }
206 
207  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion
208  */
209  void rotatePoint(const double lx,const double ly,const double lz, double &gx,double &gy,double &gz ) const
210  {
211  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();
212  const double t7 = x()*z(); const double t8 =-y()*y(); const double t9 = y()*z(); const double t10=-z()*z();
213  gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
214  gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
215  gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
216  }
217 
218  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion
219  */
220  void inverseRotatePoint(const double lx,const double ly,const double lz, double &gx,double &gy,double &gz ) const
221  {
222  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();
223  const double t7 = x()*z(); const double t8 =-y()*y(); const double t9 = y()*z(); const double t10=-z()*z();
224  gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
225  gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
226  gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
227  }
228 
229  /** Return the squared norm of the quaternion */
230  inline double normSqr() const {
231  using mrpt::utils::square;
232  return square(r()) + square(x()) + square(y()) + square(z());
233  }
234 
235  /** Normalize this quaternion, so its norm becomes the unitity.
236  */
237  inline void normalize()
238  {
239  const T qq = 1.0/std::sqrt( normSqr() );
240  for (unsigned int i=0;i<4;i++)
241  (*this)[i] *= qq;
242  }
243 
244  /** Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
245  * The output matrix can be a dynamic or fixed size (4x4) matrix.
246  */
247  template <class MATRIXLIKE>
248  void normalizationJacobian(MATRIXLIKE &J) const
249  {
250  const T n = 1.0/std::pow(normSqr(),T(1.5));
251  J.setSize(4,4);
252  J.get_unsafe(0,0)=x()*x()+y()*y()+z()*z();
253  J.get_unsafe(0,1)=-r()*x();
254  J.get_unsafe(0,2)=-r()*y();
255  J.get_unsafe(0,3)=-r()*z();
256 
257  J.get_unsafe(1,0)=-x()*r();
258  J.get_unsafe(1,1)=r()*r()+y()*y()+z()*z();
259  J.get_unsafe(1,2)=-x()*y();
260  J.get_unsafe(1,3)=-x()*z();
261 
262  J.get_unsafe(2,0)=-y()*r();
263  J.get_unsafe(2,1)=-y()*x();
264  J.get_unsafe(2,2)=r()*r()+x()*x()+z()*z();
265  J.get_unsafe(2,3)=-y()*z();
266 
267  J.get_unsafe(3,0)=-z()*r();
268  J.get_unsafe(3,1)=-z()*x();
269  J.get_unsafe(3,2)=-z()*y();
270  J.get_unsafe(3,3)=r()*r()+x()*x()+y()*y();
271  J *=n;
272  }
273 
274  /** 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$.
275  * The output matrix can be a dynamic or fixed size (4x4) matrix.
276  */
277  template <class MATRIXLIKE>
278  inline void rotationJacobian(MATRIXLIKE &J) const
279  {
280  J.setSize(4,4);
281  J.get_unsafe(0,0)=r(); J.get_unsafe(0,1)=-x(); J.get_unsafe(0,2)=-y(); J.get_unsafe(0,3)=-z();
282  J.get_unsafe(1,0)=x(); J.get_unsafe(1,1)= r(); J.get_unsafe(1,2)=-z(); J.get_unsafe(1,3)= y();
283  J.get_unsafe(2,0)=y(); J.get_unsafe(2,1)= z(); J.get_unsafe(2,2)= r(); J.get_unsafe(2,3)=-x();
284  J.get_unsafe(3,0)=z(); J.get_unsafe(3,1)=-y(); J.get_unsafe(3,2)= x(); J.get_unsafe(3,3)= r();
285  }
286 
287  /** Calculate the 3x3 rotation matrix associated to this quaternion: \f[ \mathbf{R} = \left(
288  * \begin{array}{ccc}
289  * q_r^2+q_x^2-q_y^2-q_z^2 & 2(q_x q_y - q_r q_z) & 2(q_z q_x+q_r q_y) \\
290  * 2(q_x q_y+q_r q_z) & q_r^2-q_x^2+q_y^2-q_z^2 & 2(q_y q_z-q_r q_x) \\
291  * 2(q_z q_x-q_r q_y) & 2(q_y q_z+q_r q_x) & q_r^2- q_x^2 - q_y^2 + q_z^2
292  * \end{array}
293  * \right)\f]
294  *
295  */
296  template <class MATRIXLIKE>
297  inline void rotationMatrix(MATRIXLIKE &M) const
298  {
299  M.setSize(3,3);
300  rotationMatrixNoResize(M);
301  }
302 
303  /** 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). */
304  template <class MATRIXLIKE>
305  inline void rotationMatrixNoResize(MATRIXLIKE &M) const
306  {
307  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());
308  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());
309  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();
310  }
311 
312 
313  /** Return the conjugate quaternion */
314  inline void conj(CQuaternion &q_out) const
315  {
316  q_out.r( r() );
317  q_out.x(-x() );
318  q_out.y(-y() );
319  q_out.z(-z() );
320  }
321 
322  /** Return the conjugate quaternion */
323  inline CQuaternion conj() const
324  {
325  CQuaternion q_aux;
326  conj(q_aux);
327  return q_aux;
328  }
329 
330  /** Return the yaw, pitch & roll angles associated to quaternion
331  * \sa For the equations, see The MRPT Book, or see http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
332  * \sa rpy_and_jacobian
333  */
334  inline void rpy(T &roll, T &pitch, T &yaw) const
335  {
336  rpy_and_jacobian(roll,pitch,yaw,static_cast<mrpt::math::CMatrixDouble*>(NULL));
337  }
338 
339  /** Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of the transformation.
340  * 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.
341  * \sa For the equations, see The MRPT Book, or http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
342  * \sa rpy
343  */
344  template <class MATRIXLIKE>
345  void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq = NULL, bool resize_out_dr_dq_to3x4 = true ) const
346  {
347  using mrpt::utils::square;
348  using std::sqrt;
349 
350  if (out_dr_dq && resize_out_dr_dq_to3x4)
351  out_dr_dq->setSize(3,4);
352  const T discr = r()*y()-x()*z();
353  if (fabs(discr)>0.49999)
354  { // pitch = 90 deg
355  pitch = 0.5*M_PI;
356  yaw =-2*atan2(x(),r());
357  roll = 0;
358  if (out_dr_dq) {
359  out_dr_dq->zeros();
360  out_dr_dq->get_unsafe(0,0) = +2/x();
361  out_dr_dq->get_unsafe(0,2) = -2*r()/(x()*x());
362  }
363  }
364  else if (discr<-0.49999)
365  { // pitch =-90 deg
366  pitch = -0.5*M_PI;
367  yaw =+2*atan2(x(),r());
368  roll = 0;
369  if (out_dr_dq) {
370  out_dr_dq->zeros();
371  out_dr_dq->get_unsafe(0,0) = -2/x();
372  out_dr_dq->get_unsafe(0,2) = +2*r()/(x()*x());
373  }
374  }
375  else
376  { // Non-degenerate case:
377  yaw = ::atan2( 2*(r()*z()+x()*y()), 1-2*(y()*y()+z()*z()) );
378  pitch = ::asin ( 2*discr );
379  roll = ::atan2( 2*(r()*x()+y()*z()), 1-2*(x()*x()+y()*y()) );
380  if (out_dr_dq) {
381  // Auxiliary terms:
382  const double val1=(2*x()*x() + 2*y()*y() - 1);
383  const double val12=square(val1);
384  const double val2=(2*r()*x() + 2*y()*z());
385  const double val22=square(val2);
386  const double xy2 = 2*x()*y();
387  const double rz2 = 2*r()*z();
388  const double ry2 = 2*r()*y();
389  const double val3 = (2*y()*y() + 2*z()*z() - 1);
390  const double val4 = ((square(rz2 + xy2)/square(val3) + 1)*val3);
391  const double val5 = (4*(rz2 + xy2))/square(val3);
392  const double val6 = 1.0/(square(rz2 + xy2)/square(val3) + 1);
393  const double val7 = 2.0/ sqrt(1 - square(ry2 - 2*x()*z()));
394  const double val8 = (val22/val12 + 1);
395  const double val9 = -2.0/val8;
396  // row 1:
397  out_dr_dq->get_unsafe(0,0) = -2*z()/val4;
398  out_dr_dq->get_unsafe(0,1) = -2*y()/val4;
399  out_dr_dq->get_unsafe(0,2) = -(2*x()/val3 - y()*val5)*val6 ;
400  out_dr_dq->get_unsafe(0,3) = -(2*r()/val3 - z()*val5)*val6;
401  // row 2:
402  out_dr_dq->get_unsafe(1,0) = y()*val7 ;
403  out_dr_dq->get_unsafe(1,1) = -z()*val7 ;
404  out_dr_dq->get_unsafe(1,2) = r()*val7 ;
405  out_dr_dq->get_unsafe(1,3) = -x()*val7 ;
406  // row 3:
407  out_dr_dq->get_unsafe(2,0) = val9*x()/val1 ;
408  out_dr_dq->get_unsafe(2,1) = val9*(r()/val1 - (2*x()*val2)/val12) ;
409  out_dr_dq->get_unsafe(2,2) = val9*(z()/val1 - (2*y()*val2)/val12) ;
410  out_dr_dq->get_unsafe(2,3) = val9*y()/val1 ;
411  }
412  }
413  }
414 
415  inline CQuaternion operator * (const T &factor)
416  {
417  CQuaternion q = *this;
418  q*=factor;
419  return q;
420  }
421 
422  }; // end class
423 
424  typedef CQuaternion<double> CQuaternionDouble; //!< A quaternion of data type "double"
425  typedef CQuaternion<float> CQuaternionFloat; //!< A quaternion of data type "float"
426 
427  } // end namespace
428 
429 } // end namespace mrpt
430 
431 #endif


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