SLAM14讲 #cha3 #（上）

标题 三维空间刚体运动, SLAM-cha3（上）

主要内容：

• ​ 四种运动描述：旋转矩阵、变换矩阵、四元数、欧拉角。
• ​ 学习Eigen、几何模块，Eigen主要处理矩阵变换运算。

实践步骤（ubuntu）：

一、创建.cpp文件(3种方式)

1 touch命令：touch 文件名.cpp

touch test1.cpp

2 gedit命令：gedit 文件名.cpp

gedit test.cpp

3 vim命令：vim 文件名.cpp

关于vim

按ESC键跳到命令模式，然后：

• :w - 保存文件，不退出vim。
• :w file - 将修改另外保存到file中，不退出vim。
• :w! - 强制保存，不退出vim 。
• :wq - 保存文件，退出vim。
• :wq! - 强制保存文件，退出vim。
• :q - 不保存文件，退出vim。
• :q! - 不保存文件，强制退出vim。
• :e! - 放弃所有修改，从上次保存文件开始再编辑。

二、创建cmakelist.txt文件，添加头文件

Cmake介绍：

Cmake主要编写CmakeLists.txt文件，然后用cmake命令将CmakeLists.txt文件转化为make所需要的makefile文件，最后用make命令编译源码生成可执行程序或共享库。

Cmake编译步骤：

cmake

make

cmake 指向CMakeLists.txt所在的目录，例如cmake … 表示CMakeLists.txt在当前目录的上一级目录。cmake后会生成很多编译的中间文件以及makefile文件，所以一般建议新建一个新的目录，专门用来编译，一般做法是：

mkdir build
cd build
cmake ..
make

make根据生成的makefile，编译程序。本人使用kdevelop，所以准备好文件，在kdevelop中编译。

ch3-CODE部分

#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 50

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

* 本程序演示了 Eigen 基本类型的使用
* ****************************/

int main( int argc, char** argv )

{
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Eigen::Matrix<float, 2, 3> matrix_23;  
// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
Eigen::Vector3d v_3d;     //三维向量
// 这是一样的
Eigen::Matrix<float,3,1> vd_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;
// 这种类型还有很多，我们不一一列举

// 下面是对Eigen阵的操作
// 输入数据（初始化）
matrix_23 << 1, 2, 3, 4, 5, 6;
// 输出
cout << matrix_23 << endl;

// 用()访问矩阵中的元素
for (int i=0; i<2; i++) {
    for (int j=0; j<3; j++)
        cout<<matrix_23(i,j)<<"\t";
    cout<<endl;
}

printf("-----------------------------------------------------");
printf("\n");
// 矩阵和向量相乘（实际上仍是矩阵和矩阵）
v_3d << 3, 2, 1;
vd_3d << 4,5,6;
// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
// 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;
printf("-----------------------------------------------------");
printf("\n");
Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << result2 << endl;

// 同样你不能搞错矩阵的维度
// 试着取消下面的注释，看看Eigen会报什么错
// 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;    // 行列式
printf("-----------------------------------------------------");
printf("\n");
// 特征值
// 实对称矩阵可以保证对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::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的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大

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 decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

return 0;
}

运行结果：

关于Eigen可参考学习

常用表达方式:

• 旋转矩阵（3×3）： Eigen::Matric3d。
• 旋转向量（3×1）： Eigen::AngleAxisd。
• 欧拉角（3×1）： Eigen::Vector3d。
• 四元数（4×1）： Eigen::Quaterniond。
• 欧式变换矩阵（4×4）： Eigen::Isometry3d。
• 仿射变换（4×4）： Eigen::Affine3d。
• 射影变换（4×4）： Eigen::Projective3d。

学习参考Eigen连接

https://blog.csdn.net/yxpandjay/article/details/80587916?ops_request_misc=&request_id=&biz_id=102&utm_term=Eigen&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduweb~default-0-80587916.pc_search_em_sort&spm=1018.2226.3001.4187

https://github.com/fengbingchun/Eigen_Test

可视化，安装Pangolin

• 确保已安装git

sudo apt-get install git

• 下载源码、安装依赖

apt-get install libglew-dev
apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
git clone http://github.com/stevenlovegrove/Pangolin

• 编译安装Pangolin

cd Pangolin //path of source code
mkdir build
cd build
cmake -DCPP11_NO_BOOST=1 ..
make -j
sudo make install 

• 配置完后在cmake->make, 至此build文件夹下多了一个visualizeGeometry
  如图：
  
  运行：
  
  

详细见：

【视觉SLAM十四讲 高翔】 chapte3 【视觉SLAM十四讲 高翔】_哔哩哔哩_bilibili