SLAM库系列-Eigen库：旋转矩阵、旋转向量、四元数的构造与相互转换

最新推荐文章于 2024-12-02 20:50:38 发布

十指道弦洗世尘

最新推荐文章于 2024-12-02 20:50:38 发布

阅读量381

点赞数

分类专栏： SLAM库系列文章标签：矩阵算法线性代数

本文链接：https://blog.youkuaiyun.com/weixin_57990811/article/details/129738804

版权

SLAM库系列专栏收录该内容

1 篇文章

订阅专栏

该代码示例展示了如何在C++中使用Eigen库进行旋转向量、旋转矩阵和四元数之间的转换。首先定义了一个沿Z轴旋转45度的旋转向量，然后生成对应的旋转矩阵和四元数。接着，代码演示了四种不同的转换方式：旋转矩阵到旋转向量、旋转矩阵到四元数、旋转向量到旋转矩阵以及旋转向量到四元数，最后是四元数到旋转向量和旋转矩阵的转换。

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

代码段内详细注释：

/*********************
* 目标：已知旋转向量定义为沿着Z轴旋转45°。实现旋转向量、旋转矩阵、四元数之间互相转换
*********************/
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

int main(int argc, char** argv) {
    //沿Z轴旋转45°的旋转向量
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));
    cout << "旋转向量的旋转轴 = \n" << rotation_vector.axis() << "\n 旋转向量角度 = " 
         << rotation_vector.angle() << endl;

    //沿Z轴旋转45°的旋转矩阵
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//创建单位矩阵
    rotation_matrix << 0.707, -0.707, 0,
                       0.707, 0.707,  0,
                       0,      0,     1;
    cout << "旋转矩阵 = \n" << rotation_matrix << endl;

    //沿Z轴旋转45°的四元数
    Eigen::Quaterniond quat = Eigen::Quaterniond(0, 0, 0.383, 0.924);
    cout << "四元数 = \n" << quat.coeffs() << endl;
    cout << "其中: \n x =" << quat.x() << "\n y = " << quat.y() << "\n z = " << quat.z()
         << "\n w = " << quat.w() << endl;

    //以下是互相转换
    //1. 旋转矩阵转换旋转向量
    rotation_vector.fromRotationMatrix(rotation_matrix);//方法一
    cout << "(旋转矩阵转换得到)旋转向量的旋转轴(方法一) = \n" << rotation_vector.axis() << "\n 旋转向量的角度 = "
         << rotation_vector.angle() << endl;
    rotation_vector = rotation_matrix;//方法二
    cout << "(旋转矩阵转换得到)旋转向量的旋转轴(方法二) = \n" << rotation_vector.axis() << "\n 旋转向量的角度 = "
         << rotation_vector.angle() << endl;
    rotation_vector = Eigen:AngleAxisd(rotation_matrix);//方法三
    cout << "(旋转矩阵转换得到)旋转向量的旋转轴(方法三) = \n" << rotation_vector.axis() << "\n 旋转向量的角度 = "
         << rotation_vector.angle() << endl;
    
    //2. 旋转矩阵转换四元数
    quat = Eigen:Quaterniond(rotation_matrix);//方法一
    cout << "(旋转矩阵转换得到)四元数(方法一) = \n" << quat.coeffs() << endl;
    quat = rotation_matrix;//方法二
    cout << "(旋转矩阵转换得到)四元数(方法二) = \n" << quat.coeffs() << endl;

    //3. 旋转向量转换旋转矩阵
    cout << "(旋转向量转换得到)旋转矩阵(方法一) = \n" << rotation_vector.matrix() << endl;//方法一
    cout << "(旋转向量转换得到)旋转矩阵(方法二) = \n" << rotation_vector.toRotationMatrix() << endl;//方法二

    //4. 旋转向量转换四元数
    quat = Eigen::Quaterniond(rotation_vector);
    cout << "(旋转向量转换得到)四元数 = \n" << quat.coeffs() << endl;

    //5. 四元数转换为旋转向量
    rotation_vector = quat;
    cout << "(四元数转换得到)旋转向量的旋转轴 = \n" << rotation_vector.axis() << "旋转向量的角度 = "
         << rotation_vector.angle() << endl;

    //6. 四元数转换为旋转矩阵
    rotation_matrix = quat.matrix();//方法一
    cout << "(四元数转换得到)旋转矩阵(方法一) = \n" << rotation_matrix << endl;
    rotation_matrix = quat.toRotationMatrix();//方法二
    cout << "(四元数转换得到)旋转矩阵(方法二) = \n" << rotation_matrix << endl;

    return 0;
}