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 
30 /** Calculate Yaw angle from quaternion.
31  * The Yaw angle is the rotation around the Z axis of a given reference
32  * frame.
33  * Code based on OpenSLAM.
34  * @param q quaternion to get yaw angle for
35  * @return yaw angle
36  */
37 template <typename Scalar>
38 Scalar
39 quat_yaw(const Eigen::Quaternion<Scalar> &q)
40 {
41  Scalar qx = q.x(), qy = q.y(), qz = q.z(), qw = q.w();
42  Scalar qx2 = qx * qx, qy2 = qy * qy, qz2 = qz * qz, qw2 = qw * qw;
43  // for abs(pitch) = PI/2 this will lead to atan2(0,0)
44  // i.e. for noisy values, result will be arbitrary
45  return atan2(2 * (qw * qz + qx * qy), qw2 + qx2 - qy2 - qz2);
46 }
47 
48 /** Get euler angles for quaternion.
49  * Calculates the roll, pitch, and yaw angles for a given quaternion.
50  * Code based on OpenSLAM.
51  * @param q quaternion to convert
52  * @param roll upon return contains roll angle (around X axis)
53  * @param pitch upon return contains pitch angle (around Y axis)
54  * @param yaw upon return contains yaw angle (around Z axis)
55  */
56 template <typename Scalar>
57 void
58 quat_to_euler(const Eigen::Quaternion<Scalar> &q, float &roll, float &pitch, float &yaw)
59 {
60  using std::atan2;
61  using std::cos;
62  using std::sin;
63 
64  // Get yaw angle:
65  Scalar qx = q.x(), qy = q.y(), qz = q.z(), qw = q.w();
66  Scalar qx2 = qx * qx, qy2 = qy * qy, qz2 = qz * qz, qw2 = qw * qw;
67  // for abs(pitch) = PI/2 this will lead to atan2(0,0)
68  // i.e. for noisy values, result will be arbitrary
69  yaw = atan2(2 * (qw * qz + qx * qy), qw2 + qx2 - qy2 - qz2);
70 
71  // Now rotate the original Quaternion backwards by yaw:
72  Scalar c = cos(yaw / 2), s = sin(yaw / 2);
73  Scalar px = c * qx + s * qy, py = c * qy - s * qx, pz = c * qz - s * qw, pw = c * qw + s * qz;
74  Scalar px2 = px * px, py2 = py * py, pz2 = pz * pz, pw2 = pw * pw;
75 
76  // Now calculating pitch and roll does not have singularities anymore:
77  pitch = atan2(2 * (py * pw - px * pz), px2 + pw2 - py2 - pz2);
78  roll = atan2(2 * (px * pw - py * pz), py2 + pw2 - px2 - pz2);
79 }
80 
81 } // end namespace fawkes
82 
83 #endif
Fawkes library namespace.
Scalar quat_yaw(const Eigen::Quaternion< Scalar > &q)
Calculate Yaw angle from quaternion.
Definition: eigen.h:39
void quat_to_euler(const Eigen::Quaternion< Scalar > &q, float &roll, float &pitch, float &yaw)
Get euler angles for quaternion.
Definition: eigen.h:58