Fawkes API  Fawkes Development Version
eigen.h
1 
2 /***************************************************************************
3  * eigen_utils.h - Utils related to Eigen3
4  *
5  * Created: Wed Mar 25 14:40:14 2015
6  * Copyright 2015 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 #ifndef __UTILS_MATH_EIGEN3_H_
24 #define __UTILS_MATH_EIGEN3_H_
25 
26 #include <Eigen/Geometry>
27 
28 namespace fawkes {
29 #if 0 /* just to make Emacs auto-indent happy */
30 }
31 #endif
32 
33 
34 /** Calculate Yaw angle from quaternion.
35  * The Yaw angle is the rotation around the Z axis of a given reference
36  * frame.
37  * Code based on OpenSLAM.
38  * @param q quaternion to get yaw angle for
39  * @return yaw angle
40  */
41 template <typename Scalar>
42 Scalar
43 quat_yaw(const Eigen::Quaternion<Scalar> &q)
44 {
45  Scalar qx=q.x(), qy=q.y(), qz=q.z(), qw=q.w();
46  Scalar qx2=qx*qx, qy2=qy*qy, qz2=qz*qz, qw2=qw*qw;
47  // for abs(pitch) = PI/2 this will lead to atan2(0,0)
48  // i.e. for noisy values, result will be arbitrary
49  return atan2(2*(qw*qz + qx*qy), qw2 + qx2 - qy2 - qz2);
50 }
51 
52 /** Get euler angles for quaternion.
53  * Calculates the roll, pitch, and yaw angles for a given quaternion.
54  * Code based on OpenSLAM.
55  * @param q quaternion to convert
56  * @param roll upon return contains roll angle (around X axis)
57  * @param pitch upon return contains pitch angle (around Y axis)
58  * @param yaw upon return contains yaw angle (around Z axis)
59  */
60 template <typename Scalar>
61 void
62 quat_to_euler(const Eigen::Quaternion<Scalar> &q, float &roll, float &pitch, float &yaw)
63 {
64  using std::cos;
65  using std::sin;
66  using std::atan2;
67 
68  // Get yaw angle:
69  Scalar qx=q.x(), qy=q.y(), qz=q.z(), qw=q.w();
70  Scalar qx2=qx*qx, qy2=qy*qy, qz2=qz*qz, qw2=qw*qw;
71  // for abs(pitch) = PI/2 this will lead to atan2(0,0)
72  // i.e. for noisy values, result will be arbitrary
73  yaw = atan2(2*(qw*qz + qx*qy), qw2 + qx2 - qy2 - qz2);
74 
75  // Now rotate the original Quaternion backwards by yaw:
76  Scalar c = cos(yaw/2), s=sin(yaw/2);
77  Scalar px=c*qx+s*qy, py=c*qy-s*qx, pz=c*qz-s*qw, pw=c*qw+s*qz;
78  Scalar px2=px*px, py2=py*py, pz2=pz*pz, pw2=pw*pw;
79 
80  // Now calculating pitch and roll does not have singularities anymore:
81  pitch = atan2(2*(py*pw - px*pz), px2 + pw2 - py2 - pz2);
82  roll = atan2(2*(px*pw - py*pz), py2 + pw2 - px2 - pz2);
83 }
84 
85 
86 } // end namespace fawkes
87 
88 #endif
Fawkes library namespace.
void quat_to_euler(const Eigen::Quaternion< Scalar > &q, float &roll, float &pitch, float &yaw)
Get euler angles for quaternion.
Definition: eigen.h:62
Scalar quat_yaw(const Eigen::Quaternion< Scalar > &q)
Calculate Yaw angle from quaternion.
Definition: eigen.h:43