slam十四讲：ch3之三维空间刚体运动和eigen事件总结

#include <iostream>



using namespace std;



#include <ctime>

// Eigen 核心部分

#include <Eigen/Core>

// 稠密矩阵的代数运算（逆，特征值等）

#include <Eigen/Dense>



using namespace Eigen;



#define MATRIX_SIZE 50



/****************************

* 本程序演示了 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;

  }



  // 矩阵和向量相乘（实际上仍是矩阵和矩阵）

  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;



  // 同样你不能搞错矩阵的维度

  // 试着取消下面的注释，看看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;

}

matrix 2x3 from 1 to 6: 

1 2 3

4 5 6

print matrix 2x3: 

1       2       3

4       5       6

[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.536459

 0.566198 -0.604897 -0.444451

transpose: 

 0.680375 -0.211234  0.566198

  0.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.36459

 5.66198 -6.04897 -4.44451

inverse: 

-0.198521   2.22739    2.8357

  1.00605 -0.555135  -1.41603

 -1.62213   3.59308   3.28973

det: 0.208598

Eigen values = 

0.0242899

 0.992154

  1.80558

Eigen vectors = 

-0.549013 -0.735943  0.396198

 0.253452 -0.598296 -0.760134

-0.796459  0.316906 -0.514998

time of normal inverse is 0ms

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 0ms

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 10ms

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