「 SLAM lesson-3.8 」使用Eigen表示矩阵、向量及相关计算

87 阅读 0 评论 58 点赞

我是靠谱客的博主腼腆眼神，最近开发中收集的这篇文章主要介绍「 SLAM lesson-3.8 」使用Eigen表示矩阵、向量及相关计算，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

一、前言

结合 高翔老师的著作《视觉SLAM十四讲：从理论到实践》，加上小白的工程经验共同完成。建议作为笔记功能反复使用。

二、Eigen

Eigen是一个C++开源线性代数库。相比于其他库，Eigen 特殊之处在于，它是一个纯用头文件搭建起来的库。我们在使用时，只需引入Eigen的头文件即可，不需要链接它的库文件（因为它没有库文件）。

Eigen是矩阵的基本数据单元，他是一个模板类。它的前三个参数为：数据类型、行、列。

三、头文件

#include<Eigen/Core>     //Eigen部分

#include<Eigen/Dense>    //稠密矩阵的代数运算

四、函数部分

①. 定义一个旋转矩阵

    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();  //Identity()表示单位阵

②. 定义一个旋转向量

    Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d (0,0,1));

③. 旋转向量转换成旋转矩阵（两种方式）

//方式一
    cout.precision(3);
    cout<<"rotation matrix =n"<<rotation_vector.matrix()<<endl;

//方式二
    rotation_matrix = rotation_vector.toRotationMatrix();

④. 设定一个位置坐标

    Eigen::Vector3d v(1,0,0);

⑤. 使用旋转向量进行坐标变换

    Eigen::Vector3d v_rotated = rotation_vector*v;

⑥. 使用旋转向量进行坐标变换

    v_rotated = rotation_matrix *v;

⑦. 定义一个欧拉角，并将旋转矩阵直接转换成欧拉角

    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); //ZYX顺序

⑧. 定义一个欧式变换矩阵

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();  //初始化为单位阵

⑨. 使用旋转向量旋转欧式变换矩阵

    T.rotate( rotation_vector );

⑩. 设置欧氏变换矩阵的平移向量

    T.pretranslate(Eigen::Vector3d(1,3,4));

①①. 使用欧氏变换矩阵进行坐标变换

    Eigen::Vector3d v_transformed = T*v;  //相当于 R*V+t

①②. 定义一个四元数，并直接把旋转向量赋值給四元数（反之亦然）

    Eigen::Quaterniond q = Eigen::Quaterniond( rotation_vector );

①③. 使用四元数旋转一个向量

    v_rotated = q*v;

五、完整程序

#include<iostream>
#include<cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main( int argc,char** argy )
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d (0,0,1));
    cout.precision(3);
    cout<<"rotation matrix =n"<<rotation_vector.matrix()<<endl;

    rotation_matrix = rotation_vector.toRotationMatrix();
    
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector*v;    
    cout<<"(1,0,0) after totation = "<<v_rotated.transpose()<<endl;

    v_rotated = rotation_matrix *v;
    cout<<"(1,0,0) after rotation ="<<v_rotated.transpose()<<endl;

    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate( rotation_vector );
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout<<"Transform matrix = n"<<T.matrix()<<endl;

    Eigen::Vector3d v_transformed = T*v;
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    Eigen::Quaterniond q = Eigen::Quaterniond( rotation_vector );
    cout<<"quaternion = n"<<q.coeffs()<<endl;

    v_rotated = q*v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;    
}