二、视觉SLAM所需基本知识——三维空间的刚体运动

  学习视觉SLAM需要一些基本知识，大致有：三维空间刚体运动、李群与李代数、矩阵论、优化方法、概率论。基础不牢地动山摇，而且没有数学基础的理解是片面而感性的，所以下面对视觉SLAM中所需要用到的基础知识进行分别介绍：

（1）三维空间的刚体运动

1、旋转矩阵以及变换矩阵

  一个向量可以在不同的坐标系下进行描述，假设有一个向量$a$，坐标系1和2（两个坐标系原点重合，有一定的旋转）的单位向量分别为 $(e_1,e_2,e_3)$$和$$(e_1^{{}^{\prime }},{e_2}^{{}^{\prime }},{e_3}^{{}^{\prime }})$，那么$a$可以表示为 $(e_1,e_2,e_3)\ast (a_1,a_2,a_3{)}^T$$或者$$(e_1^{{}^{\prime }},{e_2}^{{}^{\prime }},{e_3}^{{}^{\prime }})\ast ({a_1}^{{}^{\prime }},{a_2}^{{}^{\prime }},{a_3}^{{}^{\prime }}{)}^T$，二者表示同一个向量，所以有相等关系，并且可以推得：

  其中$R$为旋转矩阵，它表示一个坐标系和另外一个坐标系之间的旋转关系，该矩阵可通过数学证明为一个单位正交阵，由正交矩阵可以推得$R^T=R^{-1}$，则每行或者每列都是单位向量，且不同行或列之间正交，旋转矩阵为正交矩阵则表明对向量的变换为正交变换，正交变换可以保证变换之后的向量长度，向量之间的内积、距离和夹角等很多性质不变。（其中单位阵的含义是矩阵的行列式为单位1，根据线性代数的几何意义，线性变换是对一个向量进行线性的变换，变换前后不会产生弯曲，若变换前是直线，则变换后也是直线，矩阵的行列式表示对直线的缩放系数，由于旋转不改变向量的长度，所以旋转矩阵的行列式为1。）

  所以旋转矩阵对向量做的是一种正交变换，满足正交变换的矩阵（单位正交阵）也为旋转矩阵。

  一般的刚体运动往往由旋转和平移组成，根据欧拉定理：刚体在三维空间里的一般运动，可分解为刚体上方某一点的平移，以及绕经此点的旋转轴的转动，则刚体运动可表示为坐标系原点间的平移和绕原点间的旋转表示，用公式可表现为：$a^{{}^{\prime }}=Ra+t$，这种表达方式对于单个变换还行，但是对于复合情况有很大的不便之处，例如：
$$\begin{gathered} b=R_{1}a+t_1 \\ c=R_{2}b+t_2 \\ c=R_2(R_{1}a+t_1)+t_2 \end{gathered}$$
  为了复合变换时的方便性，将其写为齐次的形式，将旋转矩阵和平移向量放在一起，形成变换矩阵，变换矩阵的形式为：

  其中$(a,1{)}^T$为齐次坐标（用n+1维的向量表示n维空间中的点）。

2、旋转向量和欧拉角

  一个旋转只有三个自由度，但是用旋转矩阵有9个参数（三个向量分别为单位向量自由度-3，两两垂直自由度-3），所以旋转矩阵的表达是冗余的，表达效率较低，那有没有表达效率稍微高一点的呢？效率较高的有旋转向量和欧拉角，他们都只用三个量就完成了旋转的表达。

①旋转向量

  旋转向量用方向表示转轴，用长度表示转过的角度，也被称为角轴或者轴角。该表示方法只有三个变量，并且其三个量无约束，同时也更加直观，具有一定的几何意义。由于旋转向量和旋转矩阵是同一个东西的两种表达形式，它们之间有一定的换算方法，将旋转向量转化为旋转矩阵可以通过罗德里格斯公式：

  其中$R$为旋转矩阵，$I$为单位矩阵，$n$为旋转向量方向的单位向量，$\theta$为旋转角度。

将旋转矩阵转换为旋转向量的公式为：

②欧拉角

  旋转也可以分解为三个方向上的转动，转动的顺序可以不同，例如按$Z-Y-X$的顺序转动，轴也可以分为定轴和动轴，不同的领域的习惯有所不同。例如绕$Z$轴旋转可以得到偏航角$yaw$，绕旋转之后的$Y$轴旋转可以得到俯仰角$pitch$，绕旋转之后的$X$轴旋转得到横滚角$roll$。

  欧拉角也是用三个变量表示一个旋转，但是对于上述的欧拉角的表示形式，当绕$y$轴旋转90度的时候会出现万向锁的问题，此时自由度会减去1，此时欧拉角无法正确表达一个旋转。由于万向锁的问题，欧拉角不适合用于插值或者迭代，由于其较好的几何意义，常用于人机交互。并且使用三个实数表达旋转时，会不可避免的存在奇异性的问题（旋转向量也具有奇异性的问题，当转角超过360度时具有，旋转向量的长度具有周期性）。

3、四元数

  由于使用欧拉角或者旋转向量都会不可避免的遇到万向锁的问题，使用旋转矩阵又太过冗余，因此可以用一个更加紧凑且不会出现奇异性的方式来表达旋转——四元数。

  在二维的情况下，可以使用单位复数来表达一个旋转，三维情况下，四元数可以作为复数的一个扩充，四元数有一个实部和三个虚部：$q=q_0+q_{1}i+q_{2}j+q_{3}k$，其中三个虚部自己和自己的运算像复数，自己和别人的运算像叉乘。

  既然四元数能表示一个旋转，那么它是如何对空间中一个点$(x,y,z)$进行旋转的呢？

  首先将点坐标转化为一个纯虚四元数$p$，假设其经过单位四元数$q$的旋转，则旋转后的坐标用四元数可以表示为：$p^{{}^{\prime }}=qp{q}^{-1}$$，可以证明$$p^{{}^{\prime }}$为纯虚四元数，其三个坐标为旋转之后的点的坐标。并且数学上可以证明，四元数和旋转向量之间有以下的转换关系：

  四元数相对于旋转矩阵的优点是紧凑性，相对于旋转向量和欧拉角的优点是无奇异性，因此是一种有效的旋转表达方法，虽然其不太直观。