Eigen : 四元数、欧拉角、旋转矩阵相互转换

四元数->旋转矩阵

 Eigen::Quaterniond quaternion( w, x, y, z  );
 Eigen::Matrix3d rotation_matrix;
 rotation_matrix=quaternion.toRotationMatrix();

四元数->欧拉角

 Eigen::Quaterniond quaternion( w, x, y, z  );
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

旋转矩阵->四元数

Eigen::Matrix3d rotation;
    for(int i=0;i<3;i++)
    {
        rotation(0,i)=currentPoseRota(0,i);
        rotation(1,i)=currentPoseRota(1,i);
        rotation(2,i)=currentPoseRota(2,i);
    }
 Eigen::Quaterniond quaternion(rotation);
 cout<<"quaternion"<<quaternion.w()<<" "<<quaternion.x()<<"  "<<quaternion.y()<<"  "<<quaternion.z()<<endl;

旋转矩阵->欧拉角

 Eigen::Matrix3d rotation;
    for(int i=0;i<3;i++)
    {
        rotation(0,i)=currentPoseRota(0,i);
        rotation(1,i)=currentPoseRota(1,i);
        rotation(2,i)=currentPoseRota(2,i);
    }
    // 旋转矩阵to欧拉角 ZYX顺序，0表示X轴,1表示Y轴,2表示Z轴
    Eigen::Vector3d euler_angles = rotation.eulerAngles(2, 1, 0);//z-y-x
  

[注意：euler_angles(0)表示绕z轴转]

欧拉角->旋转矩阵

    Eigen::Vector3d euler(yaw,pitch,roll);;  // 对应 z y x
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitZ()) *
                      Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitX());

欧拉角->四元数

Eigen::Vector3d eulerAngle(yaw,pitch,roll);
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); 
Eigen::Quaterniond quaternion=yawAngle*pitchAngle*rollAngle;

更多请参考：https://www.cnblogs.com/lovebay/p/11215028.html