视觉SLAM十四讲(三)——三维空间刚体运动(下)

理论部分请看 :三维空间刚体运动

一、Eigen的使用

首先安装 Eigen：

sudo apt-get install libeigen3-dev

一般都安装在

/usr/include/eigen3/

中
代码：

#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){
   //声明一个 2×3 的 float 矩阵
   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();
   //如果不确定矩阵大小，可以使用动态大小的矩阵
   Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
   //更简单的
   Eigen::MatrixXd matrix_x;

   //矩阵操作
   //输入数据
   matrix_23 << 1,2,3,4,5,6;
   //输出
   cout<<"2*3矩阵 "<<matrix_23<<endl;

   //用（）访问矩阵中的元素
   for(int i = 0;i<1;i++)
      f