Eigen 四元数和欧拉角相互转换

四元数转欧拉角

Eigen::Quaterniond q(w, x, y, z);
Eigen::Matrix3d rx = q.toRotationMatrix();
Eigen::Vector3d ea = rx.eulerAngles(2,1,0);

注意：

上面按照先旋转ｘ轴(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());