2.2 視覺SLAM 實踐：Eigen

守望者与托养者發表於2024-08-12
視覺SLAM

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

相關文章