MIRA
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
YawPitchRoll.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 by
3  * MetraLabs GmbH (MLAB), GERMANY
4  * and
5  * Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
6  * All rights reserved.
7  *
8  * Contact: info@mira-project.org
9  *
10  * Commercial Usage:
11  * Licensees holding valid commercial licenses may use this file in
12  * accordance with the commercial license agreement provided with the
13  * software or, alternatively, in accordance with the terms contained in
14  * a written agreement between you and MLAB or NICR.
15  *
16  * GNU General Public License Usage:
17  * Alternatively, this file may be used under the terms of the GNU
18  * General Public License version 3.0 as published by the Free Software
19  * Foundation and appearing in the file LICENSE.GPL3 included in the
20  * packaging of this file. Please review the following information to
21  * ensure the GNU General Public License version 3.0 requirements will be
22  * met: http://www.gnu.org/copyleft/gpl.html.
23  * Alternatively you may (at your option) use any later version of the GNU
24  * General Public License if such license has been publicly approved by
25  * MLAB and NICR (or its successors, if any).
26  *
27  * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
28  * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
29  * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
30  * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
33  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
34  * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
35  * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
36  * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
37  */
38 
47 #ifndef _MIRA_YAWPITCHROLL_H_
48 #define _MIRA_YAWPITCHROLL_H_
49 
50 #ifndef Q_MOC_RUN
51 #include <boost/tuple/tuple.hpp>
52 #endif
53 
54 #include <math/Eigen.h>
55 
56 namespace mira
57 {
58 
60 
84 template<typename Derived>
90 {
91  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
92 
93  typedef typename Eigen::MatrixBase<Derived>::Index Index;
94  typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar;
95 
98  const Scalar epsilon = Eigen::NumTraits<Scalar>::dummy_precision();
99 
100 
101  const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
102  const Index i = a0;
103  const Index j = (a0 + 1 + odd)%3;
104  const Index k = (a0 + 2 - odd)%3;
105 
106  if (a0==a2)
107  {
108  Scalar s = Vector2(mat.coeff(j,i) , mat.coeff(k,i)).norm();
109  res[1] = std::atan2(s, mat.coeff(i,i));
110  if (s > epsilon)
111  {
112  res[0] = std::atan2(mat.coeff(j,i), mat.coeff(k,i));
113  res[2] = std::atan2(mat.coeff(i,j),-mat.coeff(i,k));
114  }
115  else
116  {
117  res[0] = Scalar(0);
118  res[2] = (mat.coeff(i,i)>0?1:-1)*std::atan2(-mat.coeff(k,j), mat.coeff(j,j));
119  }
120  }
121  else
122  {
123  Scalar c = Vector2(mat.coeff(i,i) , mat.coeff(i,j)).norm();
124  res[1] = std::atan2(-mat.coeff(i,k), c);
125  if (c > epsilon)
126  {
127  res[0] = std::atan2(mat.coeff(j,k), mat.coeff(k,k));
128  res[2] = std::atan2(mat.coeff(i,j), mat.coeff(i,i));
129  }
130  else
131  {
132  res[0] = Scalar(0);
133  res[2] = (mat.coeff(i,k)>0?1:-1)*std::atan2(-mat.coeff(k,j), mat.coeff(j,j));
134  }
135  }
136  if (!odd)
137  res = -res;
138  return res;
139 }
140 
155 template<typename T>
156 inline Eigen::Quaternion<T> quaternionFromYawPitchRoll(T yaw, T pitch, T roll)
157 {
158  return Eigen::Quaternion<T>(
162  );
163 }
164 
168 template<typename T>
169 inline Eigen::Quaternion<T> quaternionFromYawPitchRoll(const boost::tuples::tuple<T,T,T>& ypr)
170 {
171  return quaternionFromYawPitchRoll(ypr.template get<0>(),
172  ypr.template get<1>(),
173  ypr.template get<2>());
174 }
175 
177 
200 template<typename T>
202  const Eigen::Matrix<T,6,6>& eulerCovariance,
203  float yaw, float pitch, float roll,
204  Eigen::Quaternion<T>& oOrientation,
205  Eigen::Matrix<T,7,7>& oCovariance)
206 {
208 
209  T cy = (T)cos(yaw*(T)0.5);
210  T sy = (T)sin(yaw*(T)0.5);
211  T cp = (T)cos(pitch*(T)0.5);
212  T sp = (T)sin(pitch*(T)0.5);
213  T cr = (T)cos(roll*(T)0.5);
214  T sr = (T)sin(roll*(T)0.5);
215 
216  T ccc = cr*cp*cy;
217  T ccs = cr*cp*sy;
218  T csc = cr*sp*cy;
219  T css = cr*sp*sy;
220  T scc = sr*cp*cy;
221  T scs = sr*cp*sy;
222  T ssc = sr*sp*cy;
223  T sss = sr*sp*sy;
224 
225  J.template block<4,3>(3,3) <<
226  (T)0.5*( ssc -ccs), (T)0.5*( scs -csc), (T)0.5*( css -scc),
227  (T)0.5*(-csc -scs), (T)0.5*(-ssc -ccs), (T)0.5*( ccc +sss),
228  (T)0.5*( scc -css), (T)0.5*( ccc -sss), (T)0.5*( ccs -ssc),
229  (T)0.5*( ccc +sss), (T)0.5*(-css -scc), (T)0.5*(-csc -scs);
230 
231  oCovariance = J * eulerCovariance * J.transpose();
232  oOrientation = Eigen::Quaternion<T>(ccc+sss, scc-css, csc+scs, ccs-ssc);
233 }
234 
256 template<typename T>
258  const Eigen::Matrix<T,6,6>& eulerCovariance,
259  float yaw, float pitch, float roll)
260 {
263  quaternionCovFromYawPitchRollCov(eulerCovariance,yaw,pitch,roll,q,cov);
264  return cov;
265 }
266 
270 template<typename T>
272  const Eigen::Matrix<T,6,6>& eulerCovariance,
273  const Eigen::Quaternion<T>& rotation){
274  T yaw, pitch, roll;
275  boost::tie(yaw,pitch,roll) = quaternionToYawPitchRoll(rotation);
276  return quaternionCovFromYawPitchRollCov(eulerCovariance, yaw,pitch,roll);
277 }
278 
280 
298 template<typename T>
299 inline boost::tuples::tuple<T,T,T> quaternionToYawPitchRoll(
300  const Eigen::Quaternion<T>& q)
301 {
302  Eigen::Matrix<T,3,1> euler = eulerAngles(q.toRotationMatrix(), 2, 1, 0);
303  return boost::tuples::make_tuple(euler(0,0), euler(1,0), euler(2,0));
304 }
305 
317 template<typename T>
319  const Eigen::Matrix<T,7,7>& covariance,
320  const Eigen::Quaternion<T>& q)
321 {
323 
324  // Generally there exist 3 cases for calculation of yaw, pitch and roll
325  // from a quaternion:
326  // 1. regular case
327  // 2. gimbal lock upwards
328  // 3. gimbal lock downwards
329  // For these three cases the Jacobian is calculated
330 
331  // There exist different methods to calculate the yaw, pitch, roll angle
332  // from a quaternion:
333  //
334  // 1. The current implementation of the Jacobian is build on the calculations
335  // of yaw, pitch and roll from Eigen
336  //
337  // 2. A different calculation is described in the technical report
338  // @TECHREPORT{blanco2010se3,
339  // author = {Blanco, Jos{\'{e}}-Luis},
340  // month = sep,
341  // title = {A tutorial on SE(3) transformation parameterizations and on-manifold optimization},
342  // year = {2010},
343  // institution = {University of Malaga}
344  //}
345  // http://mapir.isa.uma.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
346  //
347  // This calculation is simpler and faster. But since we were not sure, whether
348  // it is compatible to the eigen calculation of the yaw, pitch and roll
349  // it is uncommented until yet
350 
351  // the jacobian of the eigen conversion from quaternion to yaw, pitch, roll
352  T a = (T)1.0 - (T)2.0 * (q.x() * q.x() + q.y() * q.y());
353  T b = (T)2.0 * (q.y() * q.z() + q.w() * q.x());
354  T c = sqrt(a*a + b*b);
355 
356  if(c > 1e-3)
357  {
358  // from eigen quaternionToYawPitchRoll
359  // pitch = atan2(-2.0*(q.x()*q.z()-q.w()*q.y()), c)/M_PI*180.0
360  // roll = atan2(2.0*(q.z()*q.y()+q.w()*q.x()), (1.0 - 2.0*(q.x()*q.x()+q.y() * q.y())))/M_PI*180.0
361  // yaw = atan2(2.0 * (q.y() * q.x() + q.z() * q.w()) , (1.0 - 2.0*(q.y() * q.y() + q.z() * q.z())))/M_PI*180.0
362 
363  T t1 = (T) ((T) q.y() * (T) q.y());
364  T t2 = 2 * t1;
365  T t3 = (T) q.z() * (T) q.z();
366  T t5 = 1 - t2 - 2 * t3;
367  T t7 = t5 * t5;
368  T t10 = (T) ((T) q.w() * (T) q.z() + (T) q.x() * (T) q.y());
369  T t13 = 1 / (t7 + 4 * t10 * t10);
370  T t31 = (T) q.x() * (T) q.x();
371  T t34 = 1 - 2 * t31 - 2 * t1;
372  T t35 = t34 * t34;
373  T t38 = (T) q.y() * (T) q.z() + (T) q.w() * (T) q.x();
374  T t39 = (T) (t38 * t38);
375  T t40 = 4 * t39;
376  T t42 = sqrt(t35 + t40);
377  T t47 = -(T) q.x() * (T) q.z() + (T) q.w() * (T) q.y();
378  T t48 = 2 * t47 * t38;
379  T t49 = 1 / t42;
380  T t56 = 1 / (t35 + t40 + 4 * t47 * t47);
381  T t90 = 1 - 2 * t31 - t2;
382  T t92 = t90 * t90;
383  T t95 = 1 / (t92 + 4 * t38 * t38);
384 
385  J.template block<3,4>(3,3) <<
386  2 * q.z() * t5 * t13,
387  2 * q.y() * t5 * t13,
388  (2 * q.x() * t5 + 8 * t10 * q.y()) * t13,
389  (2 * q.w() * t5 + 8 * t10 * q.z()) * t13,
390 
391  (2 * (T) q.y() * t42 - 4 * (T) t48 * (T) q.x() * t49) * t56,
392  (-2 * (T) q.z() * t42 - (T) t47 * (-8 * t34 * (T) q.x() + 8 * (T) t38 * (T) q.w()) * t49) * t56,
393  (2 * (T) q.w() * t42 - (T) t47 * (-8 * t34 * (T) q.y() + 8 * (T) t38 * (T) q.z()) * t49) * t56,
394  (-2 * (T) q.x() * t42 - 4 * (T) t48 * (T) q.y() * t49) * t56,
395 
396  2 * (T) q.x() * t90 * t95,
397  (2 * (T) q.w() * t90 + 8 * t38 * (T) q.x()) * t95,
398  (2 * (T) q.z() * t90 + 8 * t38 * (T) q.y()) * t95,
399  2 * (T) q.y() * t90 * t95;
400 
401 
402  // from technical report
403  // pitch = asin(2.0*(q.w()*q.y()-q.x()*q.z())); (differs from eigen)
404  // roll = atan2(2.0*(q.w()*q.x()+q.y()*q.z()), (1.0 - 2.0*(q.x()*q.x()+q.y()*q.y()))); (like eigen)
405  // yaw = atan2(2.0*(q.w()*q.z()+q.x()*q.y()), (1.0 - 2.0*(q.y()*q.y()+q.z()*q.z()))); (like eigen)
406 
407  //T t1 = (T) ((T) q.y() * (T) q.y());
408  //T t2 = 2 * t1;
409  //T t3 = (T) ((T) q.z() * (T) q.z());
410  //T t5 = 1 - t2 - 2 * t3;
411  //T t7 = t5 * t5;
412  //T t10 = (T) ((T) q.w() * (T) q.z() + (T) q.x() * (T) q.y());
413  //T t13 = 1 / (t7 + 4 * t10 * t10);
414  //T t31 = (T) ((T) q.w() * (T) q.w());
415  //T t38 = (T) ((T) q.x() * (T) q.x());
416  //T t42 = sqrt((T) (1 - 4 * t31 * t1 + 8 * q.w() * q.y() * q.x() * q.z() - 4 * t38 * t3));
417  //T t43 = 0.1e1 / t42;
418  //T t53 = 1 - 2 * t38 - t2;
419  //T t55 = t53 * t53;
420  //T t58 = q.w() * q.x() + q.y() * q.z();
421  //T t61 = 1 / (t55 + 4 * t58 * t58);
422  //
423  //J.template block<3,4>(3,3) <<
424  // 2 * q.z() * t5 * t13,
425  // 2 * q.y() * t5 * t13,
426  // (2 * q.x() * t5 + 8 * t10 * q.y()) * t13,
427  // (2 * q.w() * t5 + 8 * t10 * q.z()) * t13,
428  // (T) (0.2e1 * (T) q.y() * t43),
429  // -(T) (0.2e1 * (T) q.z() * t43),
430  // (T) (0.2e1 * (T) q.w() * t43),
431  // -(T) (0.2e1 * (T) q.x() * t43),
432  // 2 * q.x() * t53 * t61,
433  // (2 * q.w() * t53 + 8 * t58 * q.x()) * t61,
434  // (2 * q.z() * t53 + 8 * t58 * q.y()) * t61,
435  // 2 * q.y() * t53 * t61;
436  }
437  else{
438 
439  // for eigen
440  T t1 = q.x() * q.x();
441  T t3 = q.y() * q.y();
442  T t5 = (T)0.10e1 - (T)0.20e1 * t1 - (T)0.20e1 * t3;
443  T t6 = t5 * t5;
444  T t9 = q.y() * q.z() + q.w() * q.x();
445  T t10 = t9 * t9;
446  T t11 = (T)0.400e1 * t10;
447  T t13 = sqrt(t6 + t11);
448  T t16 = q.x() * q.z();
449  T t18 = q.w() * q.y();
450  T t20 = (T)-0.20e1 * t16 + (T)0.20e1 * t18;
451  T t22 = (T)0.1e1 / t13;
452  T t27 = t20 * t20;
453  T t33 = -t16 + t18;
454  T t45 = (T)0.1e1 / (t6 + t11 + (T)0.4e1 * t33 * t33);
455  T t67 = (T)0.2e1 * t1;
456  T t68 = (T) (q.z() * q.z());
457  T t70 = (T)0.1e1 - t67 - (T) (2 * t68);
458  T t72 = t70 * t70;
459  T t77 = (T)-0.20e1 * q.x() * q.y() + (T)0.20e1 * q.w() * q.z();
460  T t78 = t77 * t77;
461  T t80 = (T)0.1e1 / (t72 + t78);
462  T t96 = (T)0.1e1 - t67 - (T)0.2e1 * t3;
463  T t98 = t96 * t96;
464 
465  // for technical report
466  //T t1 = (T) q.w() * (T) q.w();
467  //T t2 = (T) q.x() * (T) q.x();
468  //T t4 = 1 / (T) (t1 + t2);
469 
470  if(q.x()*q.z() - q.w() * q.y() > 0){
471  // from eigen quaternionToYawPitchRoll
472  //pitch = atan2(-2.0*(q.x()*q.z()-q.w()*q.y()), c)/M_PI*180.0
473  //roll = +atan2(-2.0 * (q.x()*q.y()-q.w() * q.z()), (1.0 - 2.0*(q.x() * q.x() + q.z() * q.z())))/M_PI * 180.0
474  //yaw = 0
475 
476  J.template block<3,4>(3,3) <<
477  0,0,0,0,
478 
479  (T) ((0.20e1 * q.y() * t13 - 0.4000000000e1 * t20 * t9 * q.x() * t22) / (t6 + t11 + t27)),
480  (T) ((-0.2e1 * q.z() * t13 - t33 * (-0.80e1 * q.x() * t5 + 0.800e1 * t9 * q.w()) * t22) * t45),
481  (T) ((0.2e1 * q.w() * t13 - t33 * (-0.80e1 * t5 * q.y() + 0.800e1 * t9 * q.z()) * t22) * t45),
482  (T) ((-0.2e1 * q.x() * t13 - 0.8000000000e1 * t33 * t9 * q.y() * t22) * t45),
483 
484  (T) (0.20e1 * q.z() * t70 * t80),
485  (T) ((-0.20e1 * q.y() * t70 + 0.4e1 * t77 * q.x()) * t80),
486  -(T) (0.20e1 * q.x() * t70 / (t72 * t70 + t78)),
487  (T) (0.2e1 * q.y() * t96 / (t98 + 0.4e1 * t9 * t9));
488 
489  // from technical report
490  // pitch = -M_PI/2.0; (differs from eigen)
491  // roll = 0; (differs from eigen)
492  // yaw = 2.0 * atan2(q.x(), q.w()); (differs from eigen)
493 
494  //J.template block<3,4>(3,3) <<
495  // -2 * q.x() * t4, 2 * q.w() * t4, 0, 0,
496  // 0,0,0,0,
497  // 0,0,0,0;
498  }
499  else{
500  // from eigen quaternionToYawPitchRoll
501  //pitch = atan2(-2.0*(q.x()*q.z()-q.w()*q.y()), c)/M_PI*180.0
502  //roll = -atan2(-2.0 * (q.x()*q.y()-q.w() * q.z()), (1.0 - 2.0*(q.x() * q.x() + q.z() * q.z())))/M_PI * 180.0
503  //yaw = 0
504 
505  J.template block<3,4>(3,3) <<
506  0,0,0,0,
507 
508  (T) ((0.20e1 * q.y() * t13 - 0.4000000000e1 * t20 * t9 * q.x() * t22) / (t6 + t11 + t27)),
509  (T) ((-0.2e1 * q.z() * t13 - t33 * (-0.80e1 * q.x() * t5 + 0.800e1 * t9 * q.w()) * t22) * t45),
510  (T) ((0.2e1 * q.w() * t13 - t33 * (-0.80e1 * t5 * q.y() + 0.800e1 * t9 * q.z()) * t22) * t45),
511  (T) ((-0.2e1 * q.x() * t13 - 0.8000000000e1 * t33 * t9 * q.y() * t22) * t45),
512 
513  -(T) (0.20e1 * q.z() * t70 * t80),
514  -(T) ((-0.20e1 * q.y() * t70 + 0.4e1 * t77 * q.x()) * t80),
515  (T) (0.20e1 * q.x() * t70 / (t72 * t70 + t78)),
516  -(T) (0.2e1 * q.y() * t96 / (t98 + 0.4e1 * t9 * t9));
517 
518  // from technical report
519  // pitch = M_PI/2.0; (differs from eigen)
520  // roll = 0; (differs from eigen)
521  // yaw = -2.0 * atan2(q.x(), q.w()); (differs from eigen)
522 
523  //J.template block<3,4>(3,3) <<
524  // 2 * q.x() * t4, -2 * q.w() * t4, 0, 0,
525  // 0,0,0,0,
526  // 0,0,0,0;
527 
528  }
529  }
530 
531  return J * covariance * J.transpose();
532 }
533 
535 
550 template<typename T>
551 inline Eigen::Matrix<T,3,3> matrixFromYawPitchRoll(T yaw, T pitch, T roll)
552 {
553  typedef Eigen::Matrix<T,3,3> Mat3;
554  Mat3 Rx, Ry, Rz;
555 
556  float s1 = sin(roll);
557  float c1 = cos(roll);
558  Rx << 1, 0, 0,
559  0, c1,-s1,
560  0, s1, c1;
561 
562  float s2 = sin(pitch);
563  float c2 = cos(pitch);
564  Ry << c2, 0, s2,
565  0, 1, 0,
566  -s2, 0, c2;
567 
568  float s3 = sin(yaw);
569  float c3 = cos(yaw);
570  Rz << c3,-s3, 0,
571  s3, c3, 0,
572  0, 0, 1;
573 
574  return Rx*Ry*Rz; // operation order is from right to left
575 }
576 
581 template<typename T>
582 inline Eigen::Matrix<T,3,3> matrixFromYawPitchRoll(const boost::tuples::tuple<T,T,T>& ypr)
583 {
584  return matrixFromYawPitchRoll(ypr.template get<0>(),
585  ypr.template get<1>(),
586  ypr.template get<2>());
587 }
588 
603 template<typename T>
604 inline boost::tuples::tuple<T,T,T> matrixToYawPitchRoll(const Eigen::Matrix<T,3,3>& r)
605 {
606  Eigen::Matrix<T,3,1> euler = eulerAngles(r, 2, 1, 0);
607  return boost::tuples::make_tuple(euler(0,0), euler(1,0), euler(2,0));
608 }
609 
611 
629 template<typename T>
630 inline void derivMatricesFromYawPitchRoll(T yaw, T pitch, T roll,
631  Eigen::Matrix<T,3,3>& oR_dyaw,
632  Eigen::Matrix<T,3,3>& oR_dpitch,
633  Eigen::Matrix<T,3,3>& oR_droll)
634 {
635  typedef Eigen::Matrix<T,3,3> Mat3;
636  Mat3 Rx, Ry, Rz, dRx_droll, dRy_dpitch, dRz_dyaw;
637 
638  float s1 = sin(roll);
639  float c1 = cos(roll);
640  Rx << 1, 0, 0,
641  0, c1,-s1,
642  0, s1, c1;
643 
644  dRx_droll << 0, 0, 0,
645  0,-s1,-c1,
646  0, c1,-s1;
647 
648  float s2 = sin(pitch);
649  float c2 = cos(pitch);
650  Ry << c2, 0, s2,
651  0, 1, 0,
652  -s2, 0, c2;
653 
654  dRy_dpitch << -s2, 0, c2,
655  0, 0, 0,
656  -c2, 0,-s2;
657 
658  float s3 = sin(yaw);
659  float c3 = cos(yaw);
660  Rz << c3,-s3, 0,
661  s3, c3, 0,
662  0, 0, 1;
663 
664  dRz_dyaw << -s3,-c3, 0,
665  c3,-s3, 0,
666  0, 0, 0;
667 
668 
669  oR_dyaw = Rx*Ry*dRz_dyaw;
670  oR_dpitch = Rx*dRy_dpitch*Rz;
671  oR_droll = dRx_droll*Ry*Rz;
672 }
673 
675 
676 } // namespace mira
677 
679 
680 // injecting the methods to Eigen namespace resolves the the
681 // "unqualified lookup" problem which arises on compilers that implement
682 // the C++ standard very closely. By injecting the methods into the Eigen
683 // namespace the compiler can do argument-dependent lookup (ADL).
684 namespace Eigen {
685 
693 
694 }
695 
697 
698 #endif
void derivMatricesFromYawPitchRoll(T yaw, T pitch, T roll, Eigen::Matrix< T, 3, 3 > &oR_dyaw, Eigen::Matrix< T, 3, 3 > &oR_dpitch, Eigen::Matrix< T, 3, 3 > &oR_droll)
Returns the 3 derivates of the above rotation matrix, i.e.
Definition: YawPitchRoll.h:630
Eigen::Matrix< T, 6, 6 > quaternionCovToYawPitchRollCov(const Eigen::Matrix< T, 7, 7 > &covariance, const Eigen::Quaternion< T > &q)
Converts a 7x7 dimensional quaternion covariance (3D + Quaternion) back to a 6x6 dimensional euler co...
Definition: YawPitchRoll.h:318
Include file for all eigen related things.
boost::tuples::tuple< T, T, T > quaternionToYawPitchRoll(const Eigen::Quaternion< T > &q)
Converts a quaternion back to yaw, pitch, roll angles.
Definition: YawPitchRoll.h:299
void quaternionCovFromYawPitchRollCov(const Eigen::Matrix< T, 6, 6 > &eulerCovariance, float yaw, float pitch, float roll, Eigen::Quaternion< T > &oOrientation, Eigen::Matrix< T, 7, 7 > &oCovariance)
Converts 6x6 dimensional covariance matrix (3D + yaw, pitch and roll) angles to a 7x7 dimensional qua...
Definition: YawPitchRoll.h:201
Matrix3 toRotationMatrix(void) const
const Scalar coeff(int index) const
boost::tuples::tuple< T, T, T > matrixToYawPitchRoll(const Eigen::Matrix< T, 3, 3 > &r)
Converts a rotation matrix back to yaw, pitch, roll angles.
Definition: YawPitchRoll.h:604
Eigen::Matrix< T, 3, 3 > matrixFromYawPitchRoll(T yaw, T pitch, T roll)
Converts yaw, pitch and roll angles to a rotation matrix.
Definition: YawPitchRoll.h:551
Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 > eulerAngles(const Eigen::MatrixBase< Derived > &mat, typename Eigen::MatrixBase< Derived >::Index a0, typename Eigen::MatrixBase< Derived >::Index a1, typename Eigen::MatrixBase< Derived >::Index a2)
Returns the Euler-angles of the rotation matrix mat using the convention defined by the triplet (a0...
Definition: YawPitchRoll.h:86
Eigen::Quaternion< T > quaternionFromYawPitchRoll(T yaw, T pitch, T roll)
Converts yaw, pitch and roll angles to a quaternion.
Definition: YawPitchRoll.h:156