SLAM第一讲：旋转矩阵

1. 环境配置

安装 Eigen 

sudo apt-get install libeigen3-dev

库保存的位置 /usr/include/eigen3

安装 cmake

sudo apt-get install cmake

2. 代码介绍

#include <iostream>
#include <ctime>

using namespace std;

// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（矩阵乘法、求逆、特征值、特征向量等）
#include <Eigen/Dense>

#define MATRIX_SIZE 50

/**
 * 本程序演示了 Eigen 库的基本用法
 */

int main(int argc, char** argv)
{
    // Eigen 以矩阵为基本数据单位，他是一个模板类，他的前三个参数为：书记类型、行数、列数
    // 声明一个 2x3 矩阵，类型为 floar 型
    Eigen::Matrix<float, 2, 3> matrix_23;
    
    // 同时 Eigen 通过 typedef 提供了许多内置类型，不过底层仍是 Eigen::Matrix
    // 例如 Vector3d 实质上就是 Eigen::Matrix<double, 3, 1>
    Eigen::Vector3d v_3d;

    // 还有 Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为 0

    // 如果不确定矩阵的维度，可以使用动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 更简单的
    Eigen::MatrixXd matrix_x;
 
    // 下面是对矩阵的操作
    // 输入数据
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出数据
    cout << "matrix_23: " << endl << matrix_23 << endl;

    // 用()访问矩阵中的元素
    for(int i=0; i<1; i++)
        for(int j=0; j<2; j++)
            cout << "matrix_23(" << i << "," << j << "): " << matrix_23(i,j) << endl;

    v_3d << 1, 2, 3;
    cout << "\nv_3d: " << endl << v_3d << endl;

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵的乘法）
    // 但是在这里你不能混合俩种不同类型的矩阵，像这样是错误的
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;

    // 正确的做法是先将向量转为矩阵，然后进行乘法
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "\nresult: " << endl << result << endl;

    // 同样也不能搞错矩阵的维度
    // 试着取消下面的注释，看看会发生什么
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    // 一些矩阵运算
    matrix_33 = Eigen::Matrix3d::Random(); // 随机生成一个 3x3 矩阵
    cout << "\nmatrix_33: " << endl << matrix_33 << endl;

    cout << "\nmatrix_33.transpose(): " << endl << matrix_33.transpose() << endl; // 转置
    cout << "\nmatrix_33.sum(): " << matrix_33.sum() << endl; // 元素和
    cout << "\nmatrix_33.trace(): " << matrix_33.trace() << endl; // 迹
    cout << "\n10*matrix_33: " << endl << 10*matrix_33 << endl; // 元素乘
    cout << "\nmatrix_33.inverse(): " << endl << matrix_33.inverse() << endl; // 逆
    cout << "\nmatrix_33.determinant(): " << matrix_33.determinant() << endl; // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "\nEigenvalues: " << endl << eigen_solver.eigenvalues() << endl; // 特征值
    cout << "\nEigenvectors: " << endl << eigen_solver.eigenvectors() << endl; // 特征向量

    // 解方程
    // 求解 matrix_NN * x = v_Nd 这个方程
    // 其中 N 是矩阵的维度，d 是向量的维度
    // 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_stt1 = clock();
    Eigen::Matrix<double, MATRIX_SIZE, 1> x_direct1 = matrix_NN.inverse() * v_Nd;
    cout << "\ntime use in normal inverse: " << (double)(1000 *clock() - time_stt1) / CLOCKS_PER_SEC << "ms" << endl;

    // 通常用矩阵分解来求，例如 QR 分解，速度会快很多
    clock_t time_stt2 = clock();
    Eigen::Matrix<double, MATRIX_SIZE, 1> x_direct2 = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "\ntime use in QR composition: " << (double)(1000 *clock() - time_stt2) / CLOCKS_PER_SEC << "ms" << endl;

    return 0;
}

3. 编译程序

新建文件 CMakeLists.txt，和文件夹 build

修改 CMakeLists.txt 内容

# 声明要求的CMake版本
cmake_minimum_required(VERSION 2.8)

# 声明一个 cmake 项目
project(eigenMatrix)

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

# 添加源文件
add_executable(eigenMatrix eigenMatrix.cpp)

编译

cd ./build
cmake ..
make -j$(nproc)