视觉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++)
      for(int j = 0;j<2;j++)
          cout<<"矩阵元素: "<<matrix_23(i,j)<<endl;
   v_3d << 3,2,1;
   //矩阵和向量相乘
   //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<<"和向量相乘："<<result<<endl;

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

   //一些矩阵运算
   matrix_33 = Eigen::Matrix3d::Random();
   cout<<"矩阵运算："<<matrix_33<<endl<<endl;

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

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

   //解方程
   //求解 matrix_NN * x = v_Nd 这个方程
   //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_stt = clock();  //计时
   //直接求逆
   Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
   cout<<"time use in normal inverse is "<<1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC <<" ms"<<endl;

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

   return 0;
}

编译方法为：
在源代码所在文件夹再创建一个 CMakeLists.txt,写入：

cmake_minimum_required (VERSION 2.8)

include_directories("/usr/include/eigen3")

project(EigenMatrix)

add_executable(eigenMatrix eigenMatrix.cpp)

然后

cmake .

make

再运行就可以了

./eigenMatrix

程序中已经给出较详细注释，这里就不在解释了

二、Eigen 几何模块

代码：

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc,char** argv){
   Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
   //旋转向量使用 AngleAxis，它底层不直接是 Matrix3d，但运算可以当做矩阵(因为重载了运算符)
   Eigen::AngleAxisd rotation_vector (M_PI/4,Eigen::Vector3d(0,0,1));   //沿Z轴旋转45度
   cout .precision(3);
   cout<<"rotation matrix = 
"<<rotation_vector.matrix()<<endl; //用 matrix() 转换成矩阵
   //也可以直接赋值
   rotation_matrix = rotation_vector.toRotationMatrix();
   //用 AngleAxis 可以进行坐标变换
   Eigen::Vector3d v(1,0,0);
   Eigen::Vector3d v_rotated = rotation_vector *v;
   cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
   //或者用旋转矩阵
   v_rotated = rotation_matrix *v;
   cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

   //欧拉角：可以将旋转矩阵直接转换成欧拉角
   Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);   //ZYX 顺序，即yaw pitch roll 顺序
   cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

   //欧式变换矩阵使用 Eigen::Isometry
   Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //虽然称为3d，实质上是4×4矩阵
   T.rotate(rotation_vector);  //按照rotation_vector 进行旋转
   T.pretranslate(Eigen::Vector3d(1,3,4));  //把平移向量设成(1,3,4)
   cout<<"Transform matrix = 
"<<T.matrix()<<endl;

   //用变换矩阵进行坐标变换
   Eigen::Vector3d v_transformed = T*v;  //相当于 R*v + t
   cout<<"v transformed = "<<v_transformed.transpose()<<endl;
   //相对于仿射和射影变换，使用 Eigen::Affine3d 和Eigen::Projective3d 即可,略

   //四元数
   //可以直接把 AngleAxis 赋值给四元数，反之亦然
   Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
   cout<<"quaternion = 
"<<q.coeffs()<<endl;  //注意 coeffs 的顺序是 (x,y,z,w) ，w 为实部，前三者为虚部
   //也可以把旋转矩阵赋值给它
   q = Eigen::Quaterniond(rotation_matrix);
   cout<<"quaternion = 
"<<q.coeffs()<<endl;
   //使用四元数旋转一个向量，使用重载的乘法即可
   v_rotated = q * v;   //数学上是 qvq^{-1}
   cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

   return 0;   
}

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)

include_directories("/usr/include/eigen3")

project(UseGeometry)

add_executable(useGeometry useGeometry.cpp)

编译运行方法同上。