Eigen知识点3：Eigen旋转矩阵、旋转向量、四元素、欧拉角间的相互转换

最新推荐文章于 2022-08-31 21:22:43 发布

星辰和大海都需要门票

最新推荐文章于 2022-08-31 21:22:43 发布

阅读量1.6k

点赞数 1

分类专栏： Eigen 文章标签：自动驾驶 c++ 人工智能

本文链接：https://blog.csdn.net/qq_41545537/article/details/121379169

版权

Eigen 专栏收录该内容

5 篇文章

订阅专栏

这篇博客详细介绍了如何使用Eigen库在旋转矩阵、旋转向量、欧拉角和四元素之间进行转换。内容包括绕X、Y、Z轴的旋转，以及各种转换的示例和实验结果。此外，还涉及到四元素在物体转向中的应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

1 绕X、Y、Z轴旋转时的旋转向量为：

注意：
vector中放置的旋转轴分别为x、y、z

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

初始化旋转向量，绕X、Y、Z轴逆时针旋转pi

cout<<"绕z轴的旋转矩阵为："<<endl;
    Eigen::AngleAxisd Rotation_vector(M_PI, Eigen::Vector3d(0,0,1));
    cout<<Rotation_vector.matrix()<<endl;
    cout<< " "<<endl;
    Eigen::AngleAxisf Rotation_vector_1(M_PI,Eigen::Vector3f::UnitZ());
      cout<<Rotation_vector_1.matrix()<<endl;
    cout<< " "<<endl;

    cout<<"绕y轴的旋转矩阵为："<<endl;
    Eigen::AngleAxisd Rotation_vector1(M_PI, Eigen::Vector3d(0,1,0));
    cout<<Rotation_vector1.matrix()<<endl;
    cout<< " "<<endl;
    Eigen::AngleAxisd Rotation_vector1_1(M_PI, Eigen::Vector3d::UnitY());
     cout<<Rotation_vector1_1.matrix()<<endl;
    cout<< " "<<endl;
    

    cout<<"绕x轴的旋转矩阵为："<<endl;
    Eigen::AngleAxisd Rotation_vector2(M_PI, Eigen::Vector3d(1,0,0));
    cout<<Rotation_vector2.matrix()<<endl;
    cout<< " "<<endl;
    Eigen::AngleAxisd Rotation_vector2_1(M_PI, Eigen::Vector3d::UnitX());
     cout<<Rotation_vector2_1.matrix()<<endl;
    cout<< " "<<endl;

实验结果：
请添加图片描述

1.1 将1中的旋转向量转换为旋转矩阵形式

将上面绕x轴旋转pi的旋转向量转换为旋转矩阵形式，用罗德里格斯公式

// 将旋转向量转换为旋转矩阵
Eigen::Matrix3d Rotation_matrix=Rotation_vector2_1.matrix();
cout<<"旋转矩阵为"<<endl;
cout<<Rotation_matrix<<endl;

实验结果
请添加图片描述

1.1.1 将1.1中的旋转矩阵转旋转向量

  cout<<"绕z轴的旋转向量为："<<endl;
    Eigen::AngleAxisd Rotation_vector(M_PI, Eigen::Vector3d(0,0,1));
     // // 将旋转向量转换为旋转矩阵
    Eigen::Matrix3d Rotation_matrix=Rotation_vector.matrix();

 // 将旋转矩阵转换为旋转向量
    Eigen::AngleAxisd R_vector(Rotation_matrix);
    cout<<"旋转向量为："<<endl;
    cout<<R_vector.matrix()<<endl;

1.2 将1中的旋转向量转换为四元素形式

   // 将旋转向量转换为四元素形式
cout<<"z轴旋转向量转换为四元素形式"<<endl;
Eigen::Quaterniond q_z(Rotation_vector);
cout<<q_z.x()<<" "<<q_z.y()<<" "<<q_z.z()<<" "<<q_z.w()<<endl;

cout<<"y轴旋转向量转换为四元素形式"<<endl;
Eigen::Quaterniond q_y(Rotation_vector1);
cout<<q_y.x()<<" "<<q_y.y()<<" "<<q_y.z()<<" "<<q_y.w()<<endl;


  cout<<"x轴旋转向量转换为四元素形式"<<endl;
Eigen::Quaterniond q_x(Rotation_vector2);
cout<<q_x.x()<<" "<<q_x.y()<<" "<<q_x.z()<<" "<<q_x.w()<<endl;

请添加图片描述

1.2.1 将1.1中的旋转矩阵转换为四元素形式

cout<<"旋转矩阵转换为四元素2"<<endl;
Eigen::Quaterniond q2(Rotation_matrix);
cout<<q2.x()<<" "<<q2.y()<<" "<<q2.z()<<" "<<q2.w()<<endl;

1.3 将1中的旋转向量转换为欧拉角形式

注意：xyz分别对应RPY(0,1,2)
下边0，1，2只不过是这三个角的显示顺序不同而已
旋转向量转为欧拉角，顺序为y-z-x,也即yaw pitch roll

        // 旋转向量转换为欧拉角
    cout<<"y_旋转向量转换为欧拉角为："<<endl ;
  
    // cout<<Rotation_vector_1.matrix().eulerAngles(2,1,0)<<endl;
    cout<<" "<<endl;
cout<<Rotation_vector1.matrix().eulerAngles(1,2,0)<<endl;

等价于

rotation_matrix = AngleAxisd(euler_angles[0], Vector3f::UnitY())
     * AngleAxisd(euler_angles[1], Vector3f::UnitZ())
     * AngleAxisd(euler_angles[2], Vector3f::UnitX());

其表示先绕X轴旋转，再绕Z轴旋转，再绕Y轴旋转

注意：坐标轴方向
X轴朝前；Y轴朝左，Z轴朝右

1.3.1 将1.1中的旋转矩阵转换为欧拉角形式

代码同1.3

1.4 初始化旋转矩阵

// 初始化旋转矩阵
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix<<1,2,4,4,5,6,7,8,9;
    cout<<rotation_matrix<<endl;

请添加图片描述

1.5 欧拉角

1.5.1 初始化欧拉角

 // 初始化欧拉角
        double roll=M_PI/4,pitch=M_PI/4,yaw=M_PI/4;
        Eigen::Vector3d eulerAngle(roll,pitch,yaw);

1.5.2 欧拉角转旋转向量

        Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d(1,0,0)));
        Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d(0,0,1)));
        Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d(0,1,0)));
        Eigen::AngleAxisd rotation_vector;
        rotation_vector=yawAngle*pitchAngle*rollAngle;

实验结果：
请添加图片描述

1.5.3 欧拉角转旋转矩阵

        Eigen::Matrix3d rotation_matrix;
        rotation_matrix=yawAngle*pitchAngle*rollAngle;
         cout<<rotation_matrix<<endl;

1.5.4 欧拉角转四元素

quaternion=yawAngle*pitchAngle*rollAngle;
        cout<<"欧拉角转四元素"<<endl;
        cout<<quaternion.x()<<endl;
        cout<<quaternion.y()<<endl;
        cout<<quaternion.z()<<endl;
        cout<<quaternion.w()<<endl;

1.6 四元素

1.6.1 四元素转欧拉角

  cout<<"四元素转欧拉角"<<endl ;
    Eigen::Vector3d eulerAngle1=quaternion.matrix().eulerAngles(1,0,2);
    cout<<eulerAngle1.matrix()<<endl;

发现实验结果有问题：
输入的姿态角

double roll=M_PI/4,pitch=M_PI/4,yaw=M_PI/4;

姿态角转四元素

2 四元素知识点

实现的功能：1个物体以一定的速度转向目标物体

Quaternion.Slerp(Quaternion from ,Quaternion to, float t)叫球面线形插值

@param from 为起始方向

@param to 为终止方向

@ param t = 0.1 可以理解为速度，通常取值为0< t <= 1

3 资料参考

使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换