23 #ifndef __UTILS_MATH_EIGEN3_H_ 24 #define __UTILS_MATH_EIGEN3_H_ 26 #include <Eigen/Geometry> 41 template <
typename Scalar>
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;
49 return atan2(2*(qw*qz + qx*qy), qw2 + qx2 - qy2 - qz2);
60 template <
typename Scalar>
62 quat_to_euler(
const Eigen::Quaternion<Scalar> &q,
float &roll,
float &pitch,
float &yaw)
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;
73 yaw = atan2(2*(qw*qz + qx*qy), qw2 + qx2 - qy2 - qz2);
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;
81 pitch = atan2(2*(py*pw - px*pz), px2 + pw2 - py2 - pz2);
82 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.