使用了eigen库的格式
四元数转欧拉角
void quat_to_rpy(Quaterniond q, Matrix<double, 3, 1> &rpy)
{
// from my MATLAB implementation
// edge case!
double as = t_min2(2. * (q.w() * q.y() - q.x() * q.z()), .99999);
rpy(0) = atan2(2.f * (q.w() * q.x() + q.y() * q.z()),
1. - 2.f * (sq2(q.x()) + sq2(q.y())));
rpy(1) = asin(as);
rpy(2) = atan2(2.f * (q.w() * q.z() + q.x() * q.y()),
1. - 2.f * (sq2(q.y()) + sq2(q.z())));
}
欧拉角转四元数
Matrix<fpt, 3, 3> euler_to_rotation(fpt roll, fpt pitch, fpt yaw)
{
fpt r = roll;
fpt p = pitch;
fpt y = yaw;
// Calculate rotation matrix
Matrix<fpt, 3, 3> Rb;//Rx, Ry, Rz,
// Rx << 1, 0, 0,
// 0, cos(r), -sin(r),
// 0, sin(r), cos(r);
// Ry << cos(p), 0, sin(p),
// 0, 1, 0,
// -sin(p), 0, cos(p);
// Rb << cos(y), -sin(y), 0,
// sin(y), cos(y), 0,
// 0, 0, 1;
Rb << cos(y) * cos(p), -sin(y), 0,
sin(y) * cos(p), cos(y), 0,
-sin(p), 0, 1;
// Rb << sin(p)*sin(y) , cos(y), 0,
// sin(p) * cos(y), -sin(y), 0,
// cos(p), 0, 1;
// Rb << 1 , 0 , -sin(p) ,
// 0 , cos(r),sin(r) * cos(p),
// 0, -sin(r), cos(p) * cos(r);
Matrix<fpt, 3, 3> R =Rb.transpose();// Rb.inverse();
return R;
}