23 #ifndef _UTILS_MATH_EIGEN3_H_ 24 #define _UTILS_MATH_EIGEN3_H_ 26 #include <Eigen/Geometry> 37 template <
typename Scalar>
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;
45 return atan2(2 * (qw * qz + qx * qy), qw2 + qx2 - qy2 - qz2);
56 template <
typename Scalar>
58 quat_to_euler(
const Eigen::Quaternion<Scalar> &q,
float &roll,
float &pitch,
float &yaw)
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;
69 yaw = atan2(2 * (qw * qz + qx * qy), qw2 + qx2 - qy2 - qz2);
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;
77 pitch = atan2(2 * (py * pw - px * pz), px2 + pw2 - py2 - pz2);
78 roll = atan2(2 * (px * pw - py * pz), py2 + pw2 - px2 - pz2);
Fawkes library namespace.
void quat_to_euler(const Eigen::Quaternion< Scalar > &q, float &roll, float &pitch, float &yaw)
Get euler angles for quaternion.
Scalar quat_yaw(const Eigen::Quaternion< Scalar > &q)
Calculate Yaw angle from quaternion.