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());