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
28namespace 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 */
37template <typename Scalar>
38Scalar
39quat_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 */
56template <typename Scalar>
57void
58quat_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