高翔视觉slam十四讲书籍笔记(第三讲03-Eigen实例)

Eigen是一个 C++ 开源线性代数库。它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。许多上层的软件库也使用 Eigen 进行矩阵运算，包括 g2o、Sophus 等。照应本讲的理论部分，我们来学习一下 Eigen 的编程。

你的 PC 上可能还没有安装 Eigen。请输入以下命令来安装它：

sudo apt-get install libeigen3-dev

下面每一行的cout 打印的输出，都在上面用注释标注了
其中 matrix_33 = Eigen::Matrix3d::Random(); 生成的随机数有问题，一直是同一个矩阵

#include 
using namespace std;
#include 
// Eigen 部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main( int argc, char** argv )
{
     
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Eigen::Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量
    Eigen::Vector3d v_3d;
	// 这是一样的
    Eigen::Matrix<float,3,1> vd_3d;

    // Matrix3d 实质上是 Eigen::Matrix
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 更简单的
    Eigen::MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

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

    // 用(循环)访问矩阵中的元素
    // 1    2   3
    // 4    5   6
    for (int i=0; i<2; i++) {
     
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;
    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Eigen::Matrix result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    // 10
    // 28
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    // 32
    // 77
    cout << result2 << endl;

    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // 第一个矩阵的列数和 第二个矩阵的行数相同时，才能相乘， mxp矩阵 · pxn矩阵 = mxn矩阵
    // Eigen::Matrix result_wrong_dimension = matrix_23.cast() * v_3d;

    // 一些矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    // 此处生成的随机数矩阵，每次都是一样
    matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵 （假设生成下列的矩阵）
    // 1 2 3
    // 4 5 6
    // 7 8 9
    cout << matrix_33 << endl << endl;

    // 1 4 7
    // 2 5 8
    // 3 6 9
    cout << matrix_33.transpose() << endl;      // 转置
    // 45
    cout << matrix_33.sum() << endl;            // 各元素和
    // 一个nxn矩阵A的主对角线(从左上方至右下方的对角线)上各个元素的总和被称为矩阵的迹, 一般记作 tr(A)
    // 15
    cout << matrix_33.trace() << endl;          // 迹
    // 10 20 30
    // 40 50 60
    // 70 80 90 
    cout << 10*matrix_33 << endl;               // 数乘
    // 设A是一个n阶矩阵，若存在另一个n阶矩阵B，使得： AB=BA=E ，则称方阵A可逆，并称方阵B是A的逆矩阵
    // 1 0 -1 初等变换之后，没有逆矩阵
    // 0 1 2
    // 0 0 0 
    cout << matrix_33.inverse() << endl;        // 逆
    // 一个n×n的方阵A的行列式记为det(A)或者|A|，一个2×2矩阵的行列式可表示: 
    // det(a b) = ad - bc
    //    (c d)
    cout << matrix_33.determinant() << endl;    // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功, 自适应的特征分解，确保矩阵是自伴矩阵
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    // 计算矩阵 A 的特征值
    // f(λ) = |λ·E - A| 解得 λ 则为特征值
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    // 计算矩阵 A 的特征向量
    // 将特征值 λ 代入相应线性方程组 (λ·E - A)χ = 0， 化简求得方程组的基础解系
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大
    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;
    // 3.013ms
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
    
	// 通常用矩阵分解来求，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    // 0.06ms
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}