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

75 阅读 0 评论 50 点赞

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

概述

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

一、Eigen

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

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

二、头文件

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

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

三、函数部分

①.声明一个2*3的float矩阵

Eigen::Matrix<float,2,3>matrix_23;

②.Vector3d 实质上是 Eigen::Matrix<double,3,1>

Eigen::Vector3d v_3d;

③.Matrix3d 实质上是 Eigen::Matrix<double,3,3>

Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();  //初始化为零

④.声明一个动态矩阵（两种方式）

Eigen::Matrix< double,Eigen::Dynamic,Eigen::Dynamic > matrix_dynamic;

Eigen::MatrixXd matrix_x;

⑤.为矩阵赋值

matrix_23<< 1,2,3,4,5,6;

⑥.输出矩阵（两种方式）

cout<<matrix_23<<endl;

for(int i=0; i<1; i++)
    for(int j=0; j<2; j++)
        cout<<matrix_23(i,j)<<endl;

⑦.矩阵和向量相乘

实质上仍为矩阵与矩阵相乘

v_3d << 3,2,1;
Eigen::Matrix<double,2,1> result = matrix_23.cast<double>()*v_3d;

⑧.矩阵的基本运算

转置：

cout<<matrix_33.transpose()<<endl;

各元素和：

cout<<matrix_33.sum()<<endl;

迹：

cout<<matrix_33.trace()<<endl;

数乘：

cout<<10*matrix_33<<endl;

逆：

cout<<matrix_33.inverse()<<endl;

行列式：

cout<<matrix_33.determinant()<<endl;

特征值：

实对称矩阵可以保证对角化成功。

Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>eigen_solver(matrix_33.transpose()*matrix_33);
cout<<"Eigen values = "<<eigen_solver.eigenvalues()<<endl;
cout<<"Eigen vectors = "<<eigen_solver.eigenvectors()<<endl;

解方程：

matrix_NN * x = v_Nd

Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE>matrix_NN;
matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);

//方法一：直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x= matrix_NN.inverse()*v_Nd;

//方法二：通常用矩阵分解来求，例如 QR分解，速度会快很多
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);

四、完整的程序

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

#define MATRIX_SIZE 50

int main(int argc,char** argv)
{
    Eigen::Matrix<float,2,3>matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
    Eigen::Matrix< double,Eigen::Dynamic,Eigen::Dynamic > matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    matrix_23<< 1,2,3,4,5,6;
    cout<<matrix_23<<endl;
    for(int i=0; i<1; i++)
        for(int j=0; j<2; j++)
            cout<<matrix_23(i,j)<<endl;
    v_3d << 3,2,1;
    Eigen::Matrix<double,2,1> result = matrix_23.cast<double>()*v_3d;
    cout<< result <<endl;
    
    matrix_33 = Eigen::Matrix3d::Random();
    cout<< matrix_33<< endl<< endl;

    cout<<matrix_33.transpose()<<endl;
    cout<<matrix_33.sum()<<endl;
    cout<<matrix_33.trace()<<endl;
    cout<<10*matrix_33<<endl;
    cout<<matrix_33.inverse()<<endl;
    cout<<matrix_33.determinant()<<endl;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>eigen_solver(matrix_33.transpose()*matrix_33);
    cout<<"Eigen values = "<<eigen_solver.eigenvalues()<<endl;
    cout<<"Eigen vectors = "<<eigen_solver.eigenvectors()<<endl;

    Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE>matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);

    clock_t time_stt = clock();
    
    Eigen::Matrix<double,MATRIX_SIZE,1> x= matrix_NN.inverse()*v_Nd;
    cout<<"Time use in normal invers is "<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

       time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<<"time use in Qr compsition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

    return 0;

}

五、说明

为了实现更好的效率，在 Eigen 中你需要指定矩阵的大小和类型。
像使用 float、double那样的内置数据类型那样使用 Eigen 的矩阵，这应该是他的设计初衷。
在C++程序中，我们可以把一个 float 数据和 double 数据相加、相乘，编译器会自动把数据类型转换为最合适的那种。而在Eigen 中，出于性能的考虑，必须显示地对矩阵类型进行转换。
在计算过程中你也需要保证矩阵维数的正确性。

《视觉SLAM十四讲：从理论到实践》 PDF资源

下载链接：Robot_Starscream的资源仅供各位研究员试读，请购买纸质书籍。