專欄系列文章如下：
【視覺SLAM十四講學習筆記】第一講——SLAM介紹
【視覺SLAM十四講學習筆記】第二講——初識SLAM
【視覺SLAM十四講學習筆記】第三講——旋轉矩陣

本章將介紹視覺SLAM的基本問題之一：如何描述剛體在三維空間中的運動？

Eigen

Eigen是一個C++開源線性代數庫。它提供了快速的有關矩陣的線性代數運算，還包括解方程等功能。

請輸入以下命令進行安裝：

sudo apt-get install libeigen3-dev

與其他庫相比，Eigen的特殊之處在于，它是一個純用頭文件搭建起來的庫，這意味著你只能找到它的頭文件，而沒有類似.so或者.a的二進制文件。在使用時，只需引入Eigen的頭文件即可，不需要鏈接庫文件（因為它沒有庫文件）。接下來對這個庫的使用做一些示例：

#include <iostream>using namespace std;#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩陣的代數運算（逆，特征值等）
#include <Eigen/Dense>using namespace 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(); //初始化為零// 如果不確定矩陣大小，可以使用動態大小的矩陣Matrix<double, Dynamic, Dynamic> matrix_dynamic;// 更簡單的MatrixXd matrix_x;// 這種類型還有很多，我們不一一列舉// 下面是對Eigen陣的操作// 輸入數據（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;// 輸出cout << "matrix 2x3 from 1 to 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;}return 0;
}

輸出結果：

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3: 
1	2	3	
4	5	6	

矩陣運算：

#include <iostream>using namespace std;#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>using namespace Eigen;int main(int argc, char **argv) {// Eigen 中所有向量和矩陣都是Eigen::Matrix，它是一個模板類。它的前三個參數為：數據類型，行，列// 聲明一個2*3的float矩陣Matrix<float, 2, 3> matrix_23;Vector3d v_3d;Matrix<float, 3, 1> vd_3d;// Matrix3d 實質上是 Eigen::Matrix<double, 3, 3>Matrix3d matrix_33 = Matrix3d::Zero(); //初始化為零// 下面是對Eigen陣的操作// 輸入數據（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;// 矩陣和向量相乘（實際上仍是矩陣和矩陣）v_3d << 3, 2, 1;vd_3d << 4, 5, 6;// 但是在Eigen里你不能混合兩種不同類型的矩陣，像這樣是錯的// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;// 應該顯式轉換Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;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: " << matrix_33.sum() << endl;            // 各元素和cout << "trace: " << matrix_33.trace() << endl;          // 跡cout << "times 10: \n" << 10 * matrix_33 << endl;               // 數乘cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆cout << "det: " << matrix_33.determinant() << endl;    // 行列式// 特征值// 實對稱矩陣可以保證對角化成功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;return 0;
}	

輸出結果：

[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]: 32 77
random matrix: 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.5364590.566198 -0.604897 -0.444451
transpose: 0.680375 -0.211234  0.5661980.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.364595.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.83571.00605 -0.555135  -1.41603-1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.02428990.9921541.80558
Eigen vectors = 
-0.549013 -0.735943  0.3961980.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998

解方程：

#include <iostream>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;#define MATRIX_SIZE 50int main(int argc, char **argv) {// 解方程// 我們求解 matrix_NN * x = v_Nd 這個方程// N的大小在前邊的宏里定義，它由隨機數生成// 直接求逆自然是最直接的，但是求逆運算量大Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN= MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);matrix_NN = matrix_NN * matrix_NN.transpose();  // 保證半正定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;// 對于正定矩陣，還可以用cholesky分解來解方程time_stt = clock();x = matrix_NN.ldlt().solve(v_Nd);cout << "time of ldlt decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;return 0;
}

結果如下：

time of normal inverse is 2.606ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of Qr decomposition is 3.419ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734
time of ldlt decomposition is 1.38ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

這個例程演示了Eigen矩陣的基本操作與運算。要編譯它，需要在CMakeLists.txt里指定Eigen的頭文件目錄：

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

因為Eigen庫只有頭文件，所以不需要再用target_link_libraries語句將程序鏈接到庫上。