《视觉SLAM十四讲 第二版》笔记及课后习题（第三讲）

读书笔记：三维空间刚体运动

本讲介绍视觉 SLAM 的基本问题之一：一个刚体在三维空间中的运动是如何描述的。我们当然知道这由一次旋转加一次平移组成。平移确实没有太大问题，但旋转的处理是件麻烦事。我们将介绍旋转矩阵、四元数、欧拉角的意义,以及它们是如何运算和转换的。

旋转矩阵

一些线性代数的基本知识。
坐标系：

坐标变化：

变换矩阵：

这是一个数学技巧:我们把一个三维向量的末尾添加 1,变成了四维向量,称为齐次坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵 T 称为变换矩阵(Transform Matrix)。我们暂时用 ã 表示 a 的齐次坐标。

关于变换矩阵 T ,它具有比较特别的结构:左上角为旋转矩阵,右侧为平移向量,左下角为 0 向量,右下角为 1。这种矩阵又称为特殊欧氏群(Special Euclidean Group):

旋转向量和欧拉角

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量,称为旋转向量(或轴角, Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )表明，由于推导过程比较复杂,我们不作描述，只给出转换的结果：

符号∧是向量到反对称的转换符，反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 θ，有：
关于转轴 n,由于旋转轴上的向量在旋转后不发生改变,说明

因此,转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程,再归一化,就得到了旋转轴。读者也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。仍然剧透几句,这里的两个转换公式在下一章仍将出现,你会发现它们正是 SO(3) 上李群与李代数的对应关系。

欧拉角

ZY X 转角相当于把任意旋转分解成以下三个轴上的转角：

1. 绕物体的 Z 轴旋转,得到偏航角 yaw；
2. 绕旋转之后的 Y 轴旋转,得到俯仰角 pitch；
3. 绕旋转之后的 X 轴旋转,得到滚转角 roll。

此时,我们可以使用 [r, p, y] T 这样一个三维的向量描述任意旋转。这个向量十分的直观,我们可以从这个向量想象出旋转的过程。其他的欧拉角亦是通过这种方式,把旋转分解到三个轴上,得到一个三维的向量,只不过选用的轴,以及选用的顺序不一样。这里介绍的 rpy 角是比较常用的一种,只有很少的欧拉角种类会有 rpy 那样脍炙人口的名字。
欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)：
有关万向锁的更多详解

四元数

四元数是 Hamilton 找到的一种扩展的复数.。它既是紧凑的，也没有奇异性。如果说缺点的话，四
元数不够直观，其运算稍为复杂一些。有关四元数的部分，推荐直接看3Blue1Brown大神的四元数的可视化。我们只要掌握四元数的基本概念以及四元数到旋转矩阵的转换就行了，我们省略过程中的推导,直接给出四元数到旋转矩阵的转换方式。

实践部分

这一章要使用Eigen库，建议使用使用最新的Eigen库，因为下一章的带模板类的sophus应用需要新版本的eigen（3.3版本以上）支持，有关eigen的安装参考这篇博客。

此外，需要安装pangolin，以下是我在安装时候的错误：

用第一版的slambook的pangolin编译安装通过。

另一种解决方案是：去pangplin的官方github下载，然后按照相应步骤编译即可：
（可能是百度网盘的里的文件是之前的版本导致的吧！真是坑爹！）

useEigen：

代码没有问题，运行结果如下：

/home/wh/shenlan/slambook2/ch3/useEigen/cmake-build-debug/eigenMatrix
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 0.089ms
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.054ms
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.023ms
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 -