2.2 视觉SLAM 实践：Eigen

2.2 视觉SLAM 实践：Eigen

Eigen 是一个 C++ 开源线性代数库。它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用 Eigen 进行矩阵运算，包括 g2o、Sophus 等。

1. 安装Eigen

大部分常用的库都已经在 Ubuntu 软件源中提供，不妨先搜索 Ubuntu 的软件源是否已经提供：

rosnoetic@rosnoetic-VirtualBox:~$ sudo updatedb

rosnoetic@rosnoetic-VirtualBox:~$ locate eigen3

Eigen头文件的默认位置在“/usr/include/eigen3”中，如下所示：

如果没有安装Eigen，可以输入如下命令进行安装：

rosnoetic@rosnoetic-VirtualBox:~$ sudo apt-get install libeigen3-dev

2. 编写 eigenMatrix 函数

2.1 创建文件夹

通过终端创建一个名为eigenMatrix的文件夹以保存我们的VSCode项目，在/eigenMatrix目录下打开vscode。

rosnoetic@rosnoetic-VirtualBox:~$ mkdir -p eigenMatrix

rosnoetic@rosnoetic-VirtualBox:~$ cd eigenMatrix/

rosnoetic@rosnoetic-VirtualBox:~/eigenMatrix$ code .

2.2 编写源代码

新建文件eigenMatrix.cpp

在eigenMatrix.cpp粘贴如下代码并保存(Ctrl+S）

#include <iostream>
using namespace std;

#include <ctime>
// Eigen核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆、特征值等）
#include <Eigen/Dense>
using namespace Eigen;

#define MATRIX_SIZE 100

/*
本程序演示了Eigen基本类型的使用
*/
int main(int argc, char argv) {
    // Eigen中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为数据类型、行、列
    // 声明一个 2*3 的float矩阵
    Matrix<float, 2, 3> matrix_23;
    
    // 同时，Eigen通过Typedef提供了许多内置类型，不过底层都是Eigen::Matrix
    // 例如，Vector3d实质上是Eigen::Matrix<double,3,1>，即三维向量
    Vector3d v_3d;
    // 这是一样的
    Matrix<float, 3, 1> vd_3d;

    // Matrix3d实质上是Eigen::Matrix<double, 3, 3>
    Matrix3d matrix_33 = Matrix3d::Zero();    // 初始化为0
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;
    // 更简单的
    MatrixXd martix_x;

    // 下面是对Eigen阵的操作
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << "matrix 2x3 from 1to 6: \n" << matrix_23 << endl;

    // 用()访问矩阵中的元素
    cout << "print matrix 2x3: "<< endl;
    for (int i = 0; i <2; i++) {
        for (int j = 0; j < 3; j++) {
            cout << matrix_23(i,j) << "\t";
        }
        cout << endl;
    }

    // 矩阵和向量相乘
    v_3d << 1, 2, 3;
    vd_3d << 4, 5, 6;

    // 在Eigen里不能混合两种不同类型的矩阵，需要对类型进行显示的转换
    Matrix<double, 2, 1> result  = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6] * [1,2,3]=" << result.transpose() << endl;

    // matrix_23的数据格式为float，vd_3d的数据格式为float，所以无需进行格式转换
    Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << "[1,2,3;4,5,6] * [4,5,6]=" << result2.transpose() << endl;
                                                            
    // 矩阵运算
    // 随机数矩阵
    matrix_33 = Matrix3d::Random();
    cout << "random matrix: \n" << matrix_33 << endl;
    cout << "transpose:\n" << matrix_33.transpose() << endl;   // 转置
    cout << "sum:\n" << matrix_33.sum() << endl;  // 各元素和
    cout << "trace:\n" << matrix_33.trace() << endl;  // 迹
    cout << "times:\n" << 10*matrix_33 << endl;  // 数乘
    cout << "inverse:\n" << matrix_33.inverse() << endl;   // 逆
    cout << "det:" << matrix_33.determinant() << endl;  // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    // SelfAdjointEigenSolver是一个类，用于求解对称正定矩阵的特征值和特征向量
    SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解方程 matrix_NN * x = v_Nd
    // N的大小由前面的宏定义MATRIX_SIZE设置
    // 直接求逆自然是最直接的，但是运算量大

    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN.transpose() * matrix_NN;   // 保证半正定
    Matrix<double,MATRIX_SIZE,1> v_Nd = MatrixXd::Random(MATRIX_SIZE,1);

    clock_t time_stt = clock();   // 计时
    // 直接求逆
    Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    cout << "time of normal inverse is " << 1000 * (clock() - time_stt)/(double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x=" << x.transpose() << endl;

    // 通常采用矩阵分解法来求解，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time of Qr decomposition is " << 1000 * (clock() - time_stt)/(double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x=" << x.transpose() << endl;

    return 0;    
}

在程序撰写过程中，我们注意到了其需要使用cast进行类型转换，这是因为Eigen不支持自动类型提升，这和C++的内建数据类型有较大差异。

3. 新建 CMakeLists.txt 文件

新建CMakeLists.txt文件

在CMakeLists.txt中添加如下内容：

# 声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8 )

# 声明一个cmake工程
project(EigenMATRIX)

# 添加头文件
include_directories("/usr/include/eigen3")

# 添加一个可执行文件
add_executable(eigenMatrix eigenMatrix.cpp)

值得注意的是，与其他库相比，Eigen 的特殊之处在于，它是一个纯用头文件搭起来的库。这意味着只能找到它的头文件，而没有类似.so或.a的二进制文件。在使用时，只需引入Eigen的头文件即可，不需要链接库文件。

4. cmake 编译

ctrl+alt+T打开终端，执行如下指令进行cmake编译

rosnoetic@rosnoetic-VirtualBox:~$ cd eigenMatrix/

rosnoetic@rosnoetic-VirtualBox:~/eigenMatrix$ mkdir build

rosnoetic@rosnoetic-VirtualBox:~/eigenMatrix$ cd build/

rosnoetic@rosnoetic-VirtualBox:~/eigenMatrix/build$ cmake ..

我们新建了一个中间文件夹”build“，然后进入build文件夹，通过cmake ..命令对上一层文件夹，也就是代码所在的文件夹进行编译。这样，cmake产生的中间文件就会生成在build文件夹中，如下图所示，和源代码分开。

接着make对工程进行编译

rosnoetic@rosnoetic-VirtualBox:~/eigenMatrix/build$ make

进一步的调用可执行文件：

rosnoetic@rosnoetic-VirtualBox:~/eigenMatrix/build$ ./eigenMatrix