Eigen旋转矩阵，旋转向量，欧拉角，四元数互相转换的示例

转载注明出处。

直接放代码

/*
 * 本程序演示旋转向量，欧拉角，旋转矩阵，四元数之间的相互转换
 * Created By OuyangZizhou on 2019.5.7
 */

#include 
#include 
#include 

using namespace std;

int main()
{
    /*************分割线******************/
    // 初始化旋转向量，绕z轴旋转45°
    Eigen::AngleAxisd Rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));

    // 将旋转向量转化为旋转矩阵，用罗德里格斯公式
    Eigen::Matrix3d Rotation_matrix = Rotation_vector.matrix();

    // 旋转向量转换为四元数
    Eigen::Quaterniond q(Rotation_vector);
//    Eigen::Quaterniond q;
//    q = Rotation_vector;

    // 旋转向量转为欧拉角，顺序为z-y-x,也即yaw pitch roll
    Eigen::Vector3d eulerAngle = Rotation_vector.matrix().eulerAngles(2,1,0);

    /*************分割线******************/
    // 初始化旋转矩阵
    Eigen::Matrix3d R_matrix;
    // 以下为一种初始化方法，先直接采用上面的旋转矩阵
//    R_matrix << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    R_matrix = Rotation_matrix;

    // 旋转矩阵转化为旋转向量
    Eigen::AngleAxisd R_vector(R_matrix);
//    R_vector = R_matrix;
//    R_vector.fromRotationMatrix(R_matrix);

    // 旋转矩阵转化为四元数
    Eigen::Quaterniond q2(R_matrix);
//    q2 = R_matrix;

    // 旋转矩阵转化为欧拉角表示，欧拉角只能通过matrix表示出来
    Eigen::Vector3d eulerAngle1 = R_matrix.eulerAngles(2,1,0);

    /*************分割线******************/
    // 初始化欧拉角, yaw, pitch, roll
    Eigen::Vector3d eulerAngle2(M_PI/4,M_PI/5, M_PI/6);

    // 欧拉角转换为旋转向量，分别计算三个分量，然后将其相乘起来
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));

    Eigen::AngleAxisd rotation_vectorX;
    rotation_vectorX = yawAngle*pitchAngle*rollAngle;

    // 欧拉角转换为旋转矩阵，和旋转向量的计算同理
//    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
//    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
//    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));

    Eigen::Matrix3d R_matrix1;
    R_matrix1 = yawAngle*pitchAngle*rollAngle;

    // 欧拉角转换为四元数，和旋转向量的计算同理
    //    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
//    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
//    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond q3;
    q3 = yawAngle*pitchAngle*rollAngle;


    /*************分割线******************/
    //初始化四元数 (w,x,y,z)
    Eigen::Quaterniond q4(0.1, 0.2, 0.3, 0.4);
    q4.normalize();// 要做这一步归一化，才能用来表示旋转

    // 四元数到旋转向量
    Eigen::AngleAxisd rotation_vector3(q4);
//    rotation_vector3 = q4;
//    cout << rotation_vector3.matrix() << endl;


    // 四元数到旋转矩阵
    Eigen::Matrix3d R_matrix4(q4);
    R_matrix4 = q4;
//    cout << R_matrix4;
//    R_matrix4 = q4.matrix();
//    R_matrix4 = q4.toRotationMatrix();

    // 四元数到欧拉角（Z-Y-X）
    Eigen::Vector3d eulerAngle3 = q4.matrix().eulerAngles(2,1,0);

    return 0;
}

参考：https://blog.csdn.net/u011092188/article/details/77430988