Ubuntu下 VsCode 的 SLAM 14 讲 （ch3）

Ubuntu下 VsCode 的 SLAM 14 讲 （ch3）

三维空间刚体运动

1 Eigen库的应用

1.1 矩阵的基本运算

下面记录一下基本流程
为了成功实现《十四讲》中的“useEigen”这段代码，需要对上述配置文件进行“因地制宜”的修改。
step1 修改后的c_cpp_properties.json为：

{
    "configurations": [
        {
            "name": "Linux",
            "includePath": [
                "${workspaceFolder}/**",
                "/usr/include/",
                "/usr/local/include/",
                "/usr/include/eigen3/"
            ],
            "defines": [],
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c11",
            "cppStandard": "gnu++14",
            "intelliSenseMode": "linux-gcc-x64"
        }
    ],
    "version": 4
}

这步修改的目的是解决“找不到<Eigen/Core>文件”的问题，如：fatal error: Eigen/Core: No such file or directory。

step2 建立CMakeLists.txt, 修改为：

cmake_minimum_required(VERSION 2.8)

project(useEigen3)
project(vscode_cmake)


set(CMAKE_CXX_FLAGS "-O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
set(CMAKE_BUILD_TYPE "DEBUG")
set(SRC_LIST useEigen3.cpp)

include_directories("/usr/include/eigen3")

add_executable(useEigen3 useEigen3.cpp)
add_executable(result ${SRC_LIST})

其中，useEigen3 useEigen3.cpp是自己取的文件名，需要修改，add_executable(result ${SRC_LIST})是解决“找不到result文件”的问题。

step3 创建主函数文件“useEigen3”

#include <iostream>
using namespace std;
#include <ctime>// 计时
#include <Eigen/Core>// Eigen 核心部分
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
using namespace Eigen;
#define MATRIX_SIZE 50
/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/
int main(int argc, char **argv) {
  // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
  Matrix<float, 2, 3> matrix_23; // 声明一个2*3的float矩阵
  // 同时，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;
  }

  // 向量赋值
  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;
  //这里的取转置.transpose()只是为了表示美观
  Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
  cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

  // 另外，你不能搞错矩阵的维度，矩阵乘法（左列=右行）
  // 试着取消下面的注释，看看Eigen会报什么错
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
  // 一些矩阵运算
  // 四则运算就不演示了，直接用+-*/即可。
  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;

  // 解方程
  // 我们求解 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;
}

step4 点击左侧的运行调试（小蜘蛛），修改launch.json文件，如下所示：

{
    "version": "0.2.0",
    "configurations": [
        {
            "name": "(gdb) 启动",
            "type": "cppdbg",
            "request": "launch",
           // "program": "${workspaceFolder}/a.out",
           "program": "${workspaceFolder}/build/result",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": true,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启用整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                },
                {
                    "description":  "将反汇编风格设置为 Intel",
                    "text": "-gdb-set disassembly-flavor intel",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "CMake Build"
        }
    ]
}

step5 ctrl+shift+p 打开task：configure task->模板->Others，修改后的tasks.json如下：

{
    "options": {
       "cwd": "${workspaceFolder}/build"
    },
    "tasks": [
       {
          "label": "cmake",
          "command":"cmake",
          "args": ["-DCMAKE_BUILD_TYPE=Debug", ".."]
       },
       {
          "label": "make",
          "command":"make",
       },
       {
          "label": "CMake Build",
          "dependsOn":[
             "cmake",
             "make"
          ],
       }
    ],
    "version": "2.0.0"
 }

step6 点击下面的build，这一步是建立build文件，如果下面没有build文件，回过去看一下step2 中是否缺失 set(CMAKE_BUILD_TYPE "DEBUG")并检查一下step5是否一致 。接着ctrl+shift+b 编译。最后F5运行！