《视觉SLAM十四讲》学习笔记：第3讲 三维空间刚体运动

《视觉SLAM十四讲》学习笔记：第3讲 三维空间刚体运动

前言：本学习笔记将记录《视觉SLAM十四将》中一些重要的知识点，并对书中一些比较难的知识点添加上一些笔者个人的理解，以供笔者本人复习并与各位同学一起交流学习。本笔记结构将与原书结构一致，如果某一目录下面没有任何笔记，则代表笔者认为该小节内容相对来说没有过多重点知识。

本讲主要解决问题
理论：一个刚体在三维空间中的运动如何描述
实践：了解线性代数库Eigen

3.1 旋转矩阵

3.1.1 点、向量和坐标系
1.向量：线性空间的元素，注意只有指定了三维空间中的某个坐标系（或指定了线性空间中的一组基）时，才能够定义该向量在此坐标系下的坐标，如确定了一组基($\overrightarrow{e_1}$,$\overrightarrow{e_2}$,$\overrightarrow{e_3}$)后，就可以定义向量$\vec{a}$的坐标，如下图所示：

其中，$a_1$，$a_2$，$a_3$称为向量$\vec{a}$在基($\overrightarrow{e_1}$,$\overrightarrow{e_2}$,$\overrightarrow{e_3}$)下的坐标。
因此，向量的坐标不仅与向量本身有关，还与我们选取的坐标系有关，不同坐标系下向量的坐标可以通过旋转矩阵获得，此处不过多赘述。
2.向量的内外积：
对于$\vec{a}，\vec{b}\in R^3$，定义内积如下：

而定义外积如下：

外积的方向垂直于这两个方向，大小等于$|\vec{a}||\vec{b}|sin⟨\vec{a},\vec{b}⟩$，注意这里的符号"^"代表反对称矩阵，反对称矩阵定义为:A为反对称矩阵，如果$A^T=-A$。
3.左手系、右手系与旋转的外积表示
左手系和右手系如下图所示，目前较常用的为右手系。（部分库中仍然使用左手系）

旋转是可以通过外积来表示的，这是因为对于两个不平行的向量，如果用右手法则从一个向量指向另一个向量，大拇指所指方向为旋转向量的方向，同时也是这两个向量外积的方向。而旋转向量的大小由两个向量的夹角决定（补充一下旋转向量的定义，如果一个向量为旋转向量，其必是正交向量满足 $Q^{T}Q=I$）。旋转表示如下图所示：

3.1.2 坐标系间的欧氏变换
1.坐标变换关系：两个坐标系间的旋转与平移关系
2.坐标变换关系的引出：在机器人的运动过程中，常见的做法是设定一个惯性坐标系
（或者叫世界坐标系），可以认为它是固定不动的。同时，相机或机器人则是一个移动坐标系，那么对于相机视角中的某个向量$\vec{p}$,它在相机坐标系下的坐标为$p_c$，而该向量在世界坐标系下的坐标为$p_w$，通常，我们已知该向量在相机坐标系下的坐标$p_c$。将$p_c$转换成$p_w$即需要机器人坐标系和世界坐标系的转换关系，可以由一个矩阵$T$来描绘，如下图所示：

3.旋转矩阵与特殊正交群
相机坐标系与世界坐标系的变换称为欧氏变换，在这种变换下，同一向量在各个坐标系下的长度和不同向量之间的夹角都不会发生变化。这样的变换由旋转和平移组成。
首先考虑旋转，假设某个单位正交基($\overrightarrow{e_1}$,$\overrightarrow{e_2}$,$\overrightarrow{e_3}$)经过一组旋转变为($\overrightarrow{e_1^{{}^{\prime }}}$,$\overrightarrow{e_2^{{}^{\prime }}}$,$\overrightarrow{e_3^{{}^{\prime }}}$)，那么对于向量$\vec{a}$根据坐标的定义，有

等式两边同时左乘$\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}$，这样，左边的系数化为单位矩阵，同时得到：

将$R$称为旋转矩阵，这个矩阵由两组基的内积组成，刻画了旋转前后同一个向量的坐标变换关系。
旋转矩阵又有一个特殊的性质，旋转矩阵为行列式为1的正交矩阵，同时行列式为1的正交矩阵为旋转矩阵，因此，可以把旋转矩阵的集合定义为：

$SO(n)$为特殊正交群，通过旋转矩阵，就可以描述相机的旋转。
旋转矩阵的逆（或转置）表述了一个相反的旋转，根据定义，有：

在欧氏变换中，除了旋转之外还有一个平移。考虑世界坐标系中的向量 $\vec{a}$，经过一次
旋转（用 $R$ 描述）和一次平移$ \vec{t}$ 后，得到了$\overrightarrow{a^{{}^{\prime }}}$，那么把旋转和平移合到一起，有

其中，$\vec{t}$为平移向量。
通过上式，我们用一个旋转矩阵 $R$ 和一个平移向量 $t$ 完整地描述了一个欧氏空间的坐标变换关系。
3.1.3 变换矩阵与齐次坐标
式（3.8）虽然能够描述欧氏空间的旋转与平移，但其不是线性变换，在多次变换后形式会比较复杂，如下图所示：

因此，引入齐次坐标与变换矩阵重写式：

这是一个数学技巧：我们把一个三维向量的末尾添加 1，变成了四维向量，称为齐次
坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。这式（3.9）中称$T$为变换矩阵，称$\tilde{a}$为向量$\vec{a}$的齐次坐标。
齐次坐标通过添加最后一维，用四个实数描述一个三维向量，将变换写成线性的形式。
注意：在齐次坐标中，某个点$x$的每个分量同乘一个非零常数$k$后，仍然表示的是同一个点。因此，一个点的具体坐标值不是唯一的（详细可参考后面射影几何的内容）。但当最后一项不为零时，我们总可以把所有坐标除以最后一项，强制最后一项为1，从而得到一个点唯一的坐标表示。如下图所示：

这时，忽略掉最后一项，这个点的坐标和欧氏空间就是一样的。依靠齐次坐标和变换矩阵，两次变换的累加就是线性的了：

后面在不引起歧义的情况下，不再区分$\tilde{a}$和$\vec{a}$。
关于旋转矩阵$T$,由于其具有特殊形式，称其为特殊欧氏群。

与$SO(3)$一样，求解该矩阵的逆代表一个反向运算：

3.2 实践部分

这部分大多数重要知识在代码中，这里只讲述以下几点：
1.Eigen库只有头文件，我们不需要再用target_link_libraries语句将程序链接到库上，不过对于其他大部分库，多数时候需要用到链接命令（后面会有比较猛的find_package命令去搜索库）。
2.为了实现更好的效率，在Eigen中你需要指定矩阵的大小和类型。像旋转矩阵、变换矩阵这样的数据，完全可以在编译时期确定它们的大小和数据类型。
3.Eigen不支持数据类型混用，同时，一定注意矩阵的维数。

3.3 旋转向量和欧拉角

3.3.1 旋转向量
1.旋转矩阵的局限：1.旋转矩阵用9个量来描述一个3自由度的变换，变换矩阵利用16个量描述6自由度的变换，过于冗余。2.无论是旋转矩阵还是变换矩阵，都有正交特征进行约束，而且正交部分的行列式为1，这样当想要估计/优化旋转矩阵、变换矩阵的时候，这些约束会使求解非常复杂。
注意到对于坐标系的旋转，任意旋转都可以用一个旋转轴和一个旋转角来刻画，因此，引出旋转向量的概念。
2.用一个向量来刻画旋转，这个向量的方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量。这种表示法只需一个三维向量即可描述旋转。同样，对于变换矩阵，只需使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。
对于一个旋转轴为$\vec{n}$，角度为$\theta$的旋转，其旋转向量为$\theta \vec{n}$，旋转向量与旋转矩阵之间有如下转换关系，称为罗德里格斯公式：

再次强调符号^，向量作用该符号后得到：

由旋转矩阵也可以得到旋转向量：

这里说明一下：

因此，得到旋转角：

同时，有旋转轴上的向量在旋转后保持不变：

因此，转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程，再归一化，就得到
了旋转轴。
3.3.2 欧拉角
1.欧拉角的引入：无论是旋转矩阵、变换矩阵还是旋转向量对于人类而言都不是很直观，而欧拉角将三维空间中的旋转分解为3个绕不同坐标轴的旋转，表达直观。但由于分解方式多种多样，欧拉角的定义比较多。
2.欧拉角：经典的rpy型欧拉角表示的旋转如下图所示：

3.欧拉角的局限：欧拉角具有经典的万向节锁问题，如下图所示：

当俯仰角达到正负90度的时候，第一次旋转轴和第二次旋转轴相同，因此3维旋转实际上就成了2维旋转，由于丢失了一个自由度，将会带来很多奇异性的问题，从而使很多算法的解不唯一。详细的解不唯一的数学推导参见博客：
欧拉角奇异性产生的原因
由于欧拉角的这个局限，欧拉角不适于插值和迭代，往往只用于人机交互中。我们也很少在 SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为它具有奇异性）。不过，若你想验证自己算法是否有错时，转换成欧拉角能够快速辨认结果的正确与否。

3.4 四元数

3.4.1 四元数的定义
1.四元数的引出：可以证明，无法找到不带奇异性的三维向量描述方式来描述旋转，而利用四元数的方式来描述旋转，既紧凑，有没有奇异性。
2.四元数
四元数如下图所示：

$i,j,k为三个虚部$，这三个虚部满足这样的关系式：

同时，也可以用一个标量加一个向量的方式来表达四元数：

我们可以利用单位四元数表示三维空间中的旋转，其与旋转向量转换关系如下，假如某个旋转是绕单位向量$\vec{n}=\begin{array}{r}{n}_x,n_y,n_z\end{array}$表示，那么这个旋转的四元数形式为：

反之，也可以从单位四元数中计算出对应的旋转轴和夹角：

观察（3.19），可以发现式中$\theta$如果加上$2\pi$，将可以得到一个相同的旋转，但此时对应的四元数将由$\vec{q}$变为$\overrightarrow{-q}$，因此，在四元数中， 任意的旋转都可以由两个互为相反数的四元数表示。同理，若$\theta$取0，那么将得到没有做任何旋转的实四元数。
3.4.2 四元数的运算
四元数可以由如下各种运算，假设有两个四元数$q_a$与$q_b$:

1.加减法：

2.乘法：
乘法是把 $q_a$的每一项与 $q_b$ 每项相乘，最后相加，虚部要按照式（3.18）进行。

利用内外积的形式表达更加简洁：

注意：由于最后一项中外积的存在，两个四元数的乘法通常是不能够交换顺序的。
3.共轭：
四元数的共轭定义如下：

四元数与自己的共轭相乘，将会得到一个实数，实部为模长的平方：

4.模长
四元数模长的定义如下：

可以验证：两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四
元数

5.逆
四元数的逆定义：

按照这个定义有：

因此，对于单位四元数逆和共轭是一个量，同时乘积的逆有如下性质：

6.数乘与点乘
四元数有数乘。如下：

点乘是指两四元数每个位置上的元素相乘：

3.4.3 用四元数表示旋转
假设一个三维空间点$p=[x,y,z]\in R^3$，以及一个转轴为$\vec{n}$，夹角为$\theta$的旋转。
首先，把三维空间点用一个虚四元数表示：

同时，用四元数$\vec{q}$表示这个旋转：

那么，旋转之后该点可以表示为：

计算结果实部为0（证明的时候最好利用点乘和叉乘的形式），虚部则为旋转之后该点的坐标。

3.4.4 四元数到旋转矩阵的转换
由于推导需要一定的技巧性，此处不过多赘述。
设四元数$\vec{q}=q_0+q_1\vec{i}+q_2\vec{j}+q_3\vec{k}$，对应旋转矩阵$R$为：

反之，由旋转矩阵到四元数的转换如下。假设矩阵为$R=m_{ij},ij\in [1,2,3]$，其对应的四元数$q$由下式给出：

值得一提的是，由于 $q$ 和 $-q$ 表示同一个旋转，事实上一个$R$ 对应的四元数表示并不是惟一的。实际编程中，当 q0 接近 0 时，其余三个分量会非常大，导致解不稳定，此时我们再考虑使用其他的方式进行转换。

3.5 相似、仿射、射影变换

1.相似变换比欧氏变换多了一个自由度，它允许物体进行均匀的缩放，其矩阵表示为：

其旋转部分多了一个缩放引子$s$，因此在对向量进行旋转之后还可以进行$x,y,z$轴上均匀的缩放。
2.仿射变换
仿射变换的定义如下：

仿射变换只要求A是一个可逆矩阵，而不必是正交矩阵。仿射变换后，立方体就不再是方的了，但是各个面仍然是平行四边形。
3.射影变换
射影变换是最一般的变换，它的矩阵形式为

2D的射影变换有8个自由度，3D则共有15个自由度。从真实世界到相机照片的变换可以看成一个射影变换。

3.6 实践：Eigen几何模块

常见变换性质如下图所示：

具体操作见代码，但仍然注意单双精度不能混用。

将$d$改为$f$即可得到单精度

3.7 可视化演示

实际应用中，至少会定义两个坐标系：世界坐标系和相机坐标系。在该定义下，设某个点在世界坐标系中坐标为$p_W$，在相机坐标系下为$p_c$，那么：

或者：

实践当中使用$T_{cw}$更加常见，而$T_{wc}$更为直观。把上式的$p_c$（扩展维度之前的）取成零向量，也就是相机坐标系中的原点，此时就可以得到相机坐标系的原点在世界坐标系下的坐标：

而这正是$T_{wc}$的平移部分。因此，可以从$T_{wc}$中直接看到相机在何处。