四元数转欧拉角
Eigen::Quaterniond q(w, x, y, z);
Eigen::Matrix3d rx = q.toRotationMatrix();
Eigen::Vector3d ea = rx.eulerAngles(2,1,0);
注意:
上面按照先旋转x轴(0),然后y轴(1),最后z轴得到的角度,并不是传统意义上,zyx旋转的欧拉角。
得到的ea向量,分别对应的是rz, ry, rx旋转角度,注意和下文的顺序对应
另外这里得到的角度,归一化的范围有些问题,代码中的说明是
The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
所以,这里的rz角范围可能有问题,需要注意
欧拉角转四元数
接上面的定义,先旋转x轴,然后旋转y轴,最后旋转z轴,对应的旋转角分别为rx, ry, rz,即ea[2] ea[1] ea[0]
q = Eigen::AngleAxisd(ea[0], ::Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], ::Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], ::Eigen::Vector3d::UnitX());