C++ 四元素 欧拉角 旋转矩阵 旋转向量相互转

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;


int main(int argc, char** argv)
{
    // 定义一个四元素
    Eigen::Quaterniond q(0.8, 0.6, 0.0, 0.0);

    // 定义一个欧拉角表示RPY
    Eigen::Vector3d eulerAngle(1, 0, 0.0);

    // 定义一个旋转矩阵R
    Eigen::Matrix3d R;
    R << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;

    // 定义一个旋转向量(旋转角度和旋转轴)
    Eigen::AngleAxisd rotation_vector(3.1415926 / 4, Eigen::Vector3d(0, 0, 1));

    //【四元素】==========================================
//--1. 四元素转欧拉角
    Eigen::Vector3d eulerAngle1;
    eulerAngle1 = q.matrix().eulerAngles(2, 1, 0);

    cout << "    1. 四元素转欧拉角    "<< endl;
    cout<< eulerAngle1 << endl;




    //--2. 四元素转旋转矩阵
    Eigen::Matrix3d R1;
    R1 = q.matrix();

    cout << "    2. 四元素转旋转矩阵    " << endl;
    cout << R1 << endl;







    //--3. 四元素转旋转向量
    Eigen::AngleAxisd rotation_vector1(q);

    cout << "    3. 四元素转旋转向量    " << endl;
   


    //【欧拉角】==========================================
    //--1. 欧拉角转四元素
    Eigen::Quaterniond q1;
    q1 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());


    cout << "    1. 欧拉角转四元素   " << endl;
    cout << q1 << endl;



    //--2. 欧拉角转旋转矩阵
    //Eigen::Matrix3d R1;
    R1 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());


    cout << "   2. 欧拉角转旋转矩阵   " << endl;
    cout << R1 << endl;



    //--3. 欧拉角转旋转向量
    Eigen::AngleAxisd rotation_vector2;
    rotation_vector2 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());


    cout << "  3. 欧拉角转旋转向量  " << endl;
    cout << rotation_vector2.angle() << endl;
    cout << rotation_vector2.axis() << endl;



    //【旋转矩阵】==========================================
    //--1. 旋转矩阵转欧拉角
    Eigen::Vector3d  eulerAngle2;
    eulerAngle2 = R.eulerAngles(2, 1, 0);
    //--2. 旋转矩阵转四元素
    // 直接赋值操作
    Eigen::Quaterniond q2(R);
    //--3. 旋转矩阵转旋转向量
    Eigen::AngleAxisd rotation_vector3;
    rotation_vector3.fromRotationMatrix(R);


    cout << "  3. 旋转矩阵转旋转向量  " << endl;
    cout << rotation_vector2.angle() << endl;
    cout << rotation_vector2.axis() << endl;


    //【旋转向量】==========================================
    //--1. 旋转向量转欧拉角
    Eigen::Vector3d  eulerAngle3;
    eulerAngle2 = rotation_vector.matrix().eulerAngles(2, 1, 0);
    //--2. 旋转向量转四元素
    // 直接赋值操作
    Eigen::Quaterniond q3(rotation_vector);
    //--3. 旋转向量转旋转矩阵
    Eigen::Matrix3d R3;


    R3 = rotation_vector.matrix();
    R3 = rotation_vector.toRotationMatrix();
  

    return 0;
}