视觉 slam十四讲：CH3-Eigen基本运算

#include <ctime>
#include <eigen3/Eigen/Core>
//稠密矩阵的代数运算
#include <eigen3/Eigen/Dense>
#include <iostream>
#define MATRIX_SIZE 50

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

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

    Eigen::Vector3d v_3d;

    Eigen::Matrix<float,3,1> vd_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;

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

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


    //矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d<<3,2,1;
    vd_3d<<3,5,3;

    //注：在Eigen中不能混合两种不同类型的矩阵，如下：
//    Eigen::Matrix<double,2,1> result_wrong_type=matrix_23*v_3d;
    //应该进行显式类型转换
    Eigen::Matrix<double,2,1>result=matrix_23.cast<double>()*v_3d;
    std::cout<<"[1 2 3,4 2 5]*[3 2 1]:"<<result.transpose()<<std::endl;

    Eigen::Matrix<float,2,1>result2=matrix_23*vd_3d;
    std::cout<<"[1 2 3,4 2 5]*[3 5 3]:"<<result2.transpose()<<std::endl;

    // 不能搞错矩阵维度，可尝试取消注释，看一下Eigen会报什么错
//    Eigen::Matrix<float,2,3>result3=matrix_23*vd_3d;

    //四则运算，直接用=-*/
    matrix_33=Eigen::Matrix3d::Random();
    std::cout<<"random matrix: \n"<<matrix_33<<std::endl;

    std::cout<<"transpose: \n"<<matrix_33.transpose()<<std::endl;
    std::cout<<"sum: "<<matrix_33.sum()<<std::endl;
    std::cout<<"trace: "<<matrix_33.trace()<<std::endl;
    std::cout<<"times 10: "<<10*matrix_33<<std::endl;
    std::cout<<"inverse: "<<"\n"<<matrix_33.inverse()<<std::endl;
    std::cout<<"det: "<<matrix_33.determinant()<<std::endl;

    //特征值
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);
    std::cout<<"Eigen value: \n"<<eigen_solver.eigenvalues()<<std::endl;
    std::cout<<"Eigen Vector: \n"<<eigen_solver.eigenvectors()<<std::endl;

    //解方程
    //求解 matrix_NN * x=v_ND 方程
    // N 的大小 在前面的宏已定义,它由随机数产生
    //直接求逆自然是最直接的,但是运算量大
    Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN=
            Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    matrix_NN=matrix_NN*matrix_NN.transpose();  //保证半正定
    Eigen::Matrix<double,MATRIX_SIZE,1> 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;

    std::cout<<"time of normal inverse is:"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<std::endl;
    std::cout<<"x= "<<x.transpose()<<std::endl;

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

    //对于正定矩阵，还可以用cholesky分解求解方程
    time_stt=clock();
    x=matrix_NN.ldlt().solve(v_Nd);
    std::cout<<"time of ldlt decomposition is :"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<std::endl;
    std::cout<<"x= "<<x.transpose()<<std::endl;

    return 0;
}