《视觉SLAM》学习笔记（二）——ch3三维空间刚体运动

一、点与坐标系
二、欧式变换——旋转矩阵、.旋转向量、欧拉角、四元数
三、其他变换
四、Eigen库

课程视频地址【第二版】：https://www.bilibili.com/video/BV1D54y1G7f6
课程视频链接【第一版】：https://www.bilibili.com/video/BV16t411g7FR
课程github地址：https://github.com/gaoxiang12【第一版、第二版的代码】
电子书下载见《学习笔记（一）》1.2节

【提示】无论使用哪个版本的纸质书，先去github上对照作者给的勘误文件，纠正一下书中的错误。

《视觉SLAM十四讲》学习笔记（一）——ch1-2 预备知识，初识SLAM

《视觉SLAM十四讲》学习笔记（三）——ch4 李群李代数
《视觉SLAM十四讲》学习笔记（四）【敬请期待】
《视觉SLAM十四讲》学习笔记（五）【敬请期待】
…

Cmake官方文档：https://cmake.org/documentation/【选择自己安装的对应版本】
Eigen深入学习：https://eigen.tuxfamily.org/index.php?title=Main_Page
…

Q0：上文提到，slam问题的数学描述中位置的表达，那么如何确定Xk呢？

一、点与坐标系

（本测绘专业学渣略有些些基础。。。然后需要一些线性代数的知识，老师在视频中讲的还是比较详细的，在此不过多展开，都很基础）

2D：平面坐标+方位角

$（x,y,\theta)$

3D：空间坐标+旋转角
$（x,y,z,\alpha,\beta,\gamma)$
参考坐标系：左手系 / 右手系（多用）
相机坐标系 / 世界坐标系
向量：内积 / 外积 / 加减法

如，已知某坐标系O-XYZ（e₁ , e₂ , e₃），P点可表示为 $P=a_1 *x+a_2*y+a_3*z=(a_1 \ a_2 \ a_3)^T(x \ y \ z)$
则P点坐标可记为 (a₁ , a₂ , a₃)
Q1：如何描述坐标系之间的转换？— — 固定的世界坐标系和移动的机器人坐标系
Q2：如何计算同一向量在不同坐标系的坐标？— — 不同传感器的坐标系

坐标系之间的平移和旋转

旋转后的坐标系为O^’-X^’Y^’Z^’（e₁^’ , e₂ ^', e₃^’）， P点对应坐标变为 (a₁^’ , a₂^’ , a₃^’) 在这里插入图片描述有
$\begin{gathered} \begin{bmatrix} e_1 & e_2& e_3 \end{bmatrix}*\begin{bmatrix} a_1 \\ a_2\\ a_3 \end{bmatrix}=\begin{bmatrix} e_1^‘ & e_2^‘&e_3^‘ \end{bmatrix}*\begin{bmatrix} a_1^‘ \\ a_2^‘\\ a_3^‘ \end{bmatrix} \end{gathered}$

得到旋转矩阵R。

二、欧式变换——旋转矩阵、.旋转向量、欧拉角、四元数

欧式变换是保持向量的长度和角度不变，是最简单的变换，是对刚体进行了移动和旋转。

2.1 旋转矩阵

满足：R是一个正交矩阵（逆=转置），R的行列式值为+1的矩阵称为旋转矩阵。

满足该矩阵的集合称为：Special Orthogonal Group特殊正交群，n是维度。

旋转+平移： $a^‘=R a+t$
可写成，称为齐次坐标

满足该矩阵的集合称为：Special Euclidean Group特殊欧氏群，左上角是旋转矩阵，右侧为平移向量左下角是0向量。

该矩阵的逆为

2.2 旋转向量

旋转矩阵9个参数，有正交约束和行列式约束；角轴（李代数）3个参数，没有约束——同一事物的不同描述。
罗德里格斯公式（Rodrigues）：一个向量绕旋转轴旋转给定角度以后得到的新向量的计算公式，刚体运动基本计算式——任意旋转都可以用一个旋转轴和一个旋转角来表示（Axis-Angle)。

轴角：θ

罗德里格斯公式（Rodrigues）推导：
在这里插入图片描述

角度： $\theta=arccos(\frac {tr(R)-1} {2})$
轴： $R n = n$

2.3 欧拉角（Euler Angles）

欧拉角将旋转分解成三次不同轴上的转动，轴可以是定轴也可以是动轴，存在多种定义方式。（右手系）

欧拉角有两种：

【静态】：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态。
【动态】：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。

在这里插入图片描述
常见的是：偏航角(yaw)-俯仰角(pitch)-滚转角(roll)
绕Z轴转——偏航角(yaw)，绕Y轴转——俯仰角(pitch)，绕X轴转——滚转角(roll)。

不经常使用欧拉角，因为存在“万向锁”的问题。

【万向锁】：第二次旋转 [绕Y轴（pitch角）] 为±90°时，会导致第一次旋转和第三次旋转等价，旋转系统丢失了一个表示维度（自由度），旋转受限，只能绕竖直轴。

在这里插入图片描述

2.4 四元数（Quaternion）

2.4.1 四元数结构

【手敲公式真是…跪了…】

一种扩展性复数，1个实部(s)+3个虚部(v)，可以表达三维空间的旋转。
$q=q_0+q_1i+q_2j+q_3k$
$s=q_0\in\mathbb R ,v=[ q_1,q_2,q_3]^T\in\mathbb R^3$
虚部之间的关系：

2.4.2 四元数表示旋转

假设某个旋转是绕单位向量 n 进行了角度为θ的旋转：
$n=[n_x,n_y,n_z]^T$
那么这个旋转四元数的形式为（该公式也即轴角到四元数的转换）：
$\frac {\theta} {2} ,n_xsin \frac {\theta} {2} ,n_ycos \frac {\theta} {2} ,n_zsin \frac {\theta} {2} ] =[cos \frac {\theta} {2} ,n sin \frac {\theta} {2} ]$
q是单位四元数，也是纯4元数，模长等于1。

从四元数反算轴角：
在这里插入图片描述

2.4.3 四元数表示一个空间点P及其旋转

用实部为0的虚四元数表示空间点，四元数的三个虚部对应空间三个轴，
$p = [0, x, y, z] = [0, v]$
将起旋转后得到P’，有如下关系（P’也是虚四元数）：
$p'=qpq^{-1}$

2.4.4 四元数运算

在这里插入图片描述

模长

两个四元数乘积的模等于模的乘积。
$\lVert q_aq_b\rVert =\lVert q_a\rVert \lVert q_b\rVert$
共轭
四元数的共轭是对虚部取相反数
逆
$q^{-1} = q^* / \lVert q\rVert ^2$
四元数和自己的逆的乘积等于1
$qq^{-1} = q^{-1} q=1$
加减法
乘法
每项相乘再相加

向量乘积的形式

四元数的乘法通常是不可交换的，除非两个四元数在三维空间中共线。
数乘
点乘
欧几里得内积，可交换积。
$q_a \cdot q_b = s_a s_b +x_a x_bi+y_a y_bj+z_a z_bk$

2.5 区别与联系

类别	参数个数	特点
旋转矩阵	9个量描述3个自由度的旋转	冗余，有正交约束和行列式约束
旋转向量	2个量描述	紧凑，具有奇异性，无约束
欧拉角	3个量描述	紧凑，具有奇异性
四元数	个量描述	紧凑，没有奇异性，运算复杂

彼此都可以相互转换

三、其他变换

3.1 相似变换

相似变换改变了物体的大小，相对于欧式变换多了一个均匀缩放的自由度，是在旋转部分增加了一个缩放因子s。
$T_s= \begin{bmatrix} sR & T \\ 0^T & 1 \\ \end{bmatrix}$

3.2 仿射变换

仿射变换，又称仿射映射、正交投影，是指在几何中，一个向量空间进行一次线性变换并接上一个平移，变换为另一个向量空间。
$T_A= \begin{bmatrix} A & T \\ 0^T & 1 \\ \end{bmatrix}$
A可逆不必正交。

3.3 射影变换

射影变换是最一般的变换，遵循现实世界近大远小的规则。2D射影变换有8个自由度，3D射影变换有15个自由度。
$T_P= \begin{bmatrix} A & T \\ a^T & v \\ \end{bmatrix}$
使用了齐次坐标，可逆矩阵A、平移t，缩放aT。

3.4 区别与联系

在这里插入图片描述

四、Eigen库

4.1 Eigen库简介及安装

Eigen是可以用来进行线性代数、矩阵、向量操作等运算的C++库。Eigen是一个用纯头文件搭建起来的库，使用时只需引入Eigen的头文件，不需要链接库文件（也没有库文件）。

安装：sudo apt-get install libeigen3-dev
查看版本：locate eigen3 先看一下安装位置，一般都在/usr/include/eigen3或者/usr/local/include/eigen3，然后cat /usr/include/eigen3/Eigen/src/Core/util/Macros.h，打开这个Macros.h就能看到了，使用gedit打开也行。如图，我的是3.2.92版本。

4.2 Eigen库初识

【书3.2节内容】
Eigen深入学习：https://eigen.tuxfamily.org/index.php?title=Main_Page

【useEigen文件夹】
书中也给了相应的练习代码，一起手敲一遍吧～～或者可以自己尝试编写一些语句，试用一下求逆、转置、求行列式等功能。
套路，新建useEigen文件夹，还是先写CMakeLists.txt文件和eigenMatrix.cpp文件，然后cmake和make。

如果内部构建就在CMakeLists.txt同级文件夹下cmake .，不推荐，生成的文件较多，很乱；
如果外部构建就是新建build文件夹，使用cmake ..，生成的文件都放在build下，更清晰一些。

使用不同的工具，稍有不同。vscode和Clion都会自动生成外部编译文件夹。

使用终端：

#进入useEigen文件夹
~/code/C++/ch3/useEigen$ mkdir build  #新建build文件夹
~/code/C++/ch3/useEigen$ mkdir build && cd build   #新建并进入build文件夹
~/code/C++/ch3/useEigen/build$ cmake ..  #编译，生成Makefile文件
~/code/C++/ch3/useEigen/build$ ls #查看一下生成的文件
CMakeCache.txt  CMakeFiles  cmake_install.cmake  Makefile
~/code/C++/ch3/useEigen/build$ make #生成可执行文件eigenMatrix
~/code/C++/ch3/useEigen/build$ ./eigenMatrix #执行eigenMatrix文件

tree命令查看一下生成的文件结构
在这里插入图片描述

使用Clion / VScode ：
Clion和VScode都会自动生成外部编译文件夹。
Clion自动生成cmake-build-debug文件夹，VScode自动生成build文件。

以Clion为例，打开useEigen文件夹，自动生成cmake-build-debug文件夹。
在这里插入图片描述
在cmake-build-debug文件夹下进行编译。
tree命令查看一下生成的文件结构

3. 运行结果：

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 3.382ms
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 0.071ms
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 0.047ms
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

4.3 Eigen库几何模块练习

【书3.6节内容】
在这里插入图片描述
【useGeometry文件夹】
老规矩，敲一遍代码。注意Eigen输出的四元数运算结果中实部与虚部的位置，一般虚部在前。

4.4 可视化模块练习

【书3.6节内容】
【visualizeGeometry文件夹】
这个模块使用了pangolin，所以需要先安装一下，否则会报错。
Pangolin是对OpenGL进行封装的轻量级的OpenGL输入/输出和视频显示的库。可以用于3D视觉和3D导航的视觉图，可以输入各种类型的视频、并且可以保留视频和输入数据用于debug。
安装过程高翔老师在github相应位置的readme文档做了说明，从下载到安装的过程见4.5.2部分。

编译好打开可执行文件，可以在右侧拖动旋转三色方块的位置、缩放其大小。左边窗口显示的是实时的旋转矩阵（ t 改变）。

在这里插入图片描述

4.5 问题及解决

4.5.1 “can not find any cmake profile”

File -> Settings -> Build, Ececution, Deploymend -> CMake
在这里插入图片描述

4.5.2 “ Could not find a package configuration file provided by ‘Pangolin’ with any of the following names“

没安装"Pangolin"。
下载地址：https://github.com/stevenlovegrove/Pangolin

sudo apt-get install libglew-dev #安装依赖项
#下载Pangolin安装包，我得到了”Pangolin-master.zip“压缩文件
cd [path-to-pangolin]/  #进入压缩文件的路径 
unzip Pangolin-master.zip #解压xxxx.zip文件
cd Pangolin-master/  #进入该文件夹
mkdir build && cd build #新建并进入build
cmake ..
make
sudo make install

4.5.3 “./visualizeGeometry: /lib/x86_64-linux-gnu/libz.so.1: version `ZLIB_1.2.9’ not found”

软链接的问题。zlib是提供数据压缩用的函式库。
参考：https://stackoverflow.com/questions/48306849/lib-x86-64-linux-gnu-libz-so-1-version-zlib-1-2-9-not-found/51317504
安装包下载：https://sourceforge.net/projects/libpng/files/latest/download

tar -xvf ~/Downloads/zlib-1.2.9.tar.gz  #解压.tar.gz包
cd zlib-1.2.9
sudo -s 
./configure; make; make install #安装
cd /lib/x86_64-linux-gnu
ln -s -f /usr/local/lib/libz.so.1.2.9/lib libz.so.1 #创建链接
cd ~
rm -rf zlib-1.2.9

下个笔记见咯～