【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵

专栏系列文章如下：
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM

本章将介绍视觉SLAM的基本问题之一：如何描述刚体在三维空间中的运动？

旋转矩阵

点、向量和坐标系

三维空间由3个轴组成，所以一个空间点的位置可以由3个坐标指定。而刚体，它不光有位置，还有自身的姿态。举个例子，相机也可以看成三维空间的刚体，于是位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向，合起来称为位姿。

点： 点是空间中的基本元素，没有长度，没有体积。

向量： 把两个点连接起来，就构成了向量。向量可以看成从某点指向另一点的一个箭头。不要把向量与它的坐标两个概念混淆。一个向量是空间当中的一样东西，并不需要和若干个实数相关联的。只有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是找到若干个实数对应这个向量。简单来讲：向量本身就是一个东西，如果把它放在一个坐标系中，这个向量才有了对应于这个坐标系的坐标。

用线性代数的知识来说，三维空间中的某个点的坐标也可以用线性空间R^3来描述。假设在这个线性空间内，有该空间的一组基 （或者说基底）(e1,e2,e3)，（基就是张成这个空间的一组线性相关的向量）那么，任意向量a在这组基下就有一个坐标，这里 (a1,a2,a3)T 称为a在此基下的坐标。公式如下：

坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。坐标系通常由3个正交的坐标轴组成，非正交的很少见。通常使用右手系，给定x和y轴时，z 轴就可以通过右手法则由x × y定义出来。左手系的第3个轴与右手系的方向相反。

向量内积： 可以描述向量间的投影关系

其中<a,b>指向量a,b的夹角。

向量外积：

外积的结果是一个向量，它的方向垂直于这两个向量，大小为 |a||b|sin〈a,b〉，是两个向量张成的四边形的有向面积。

对于外积运算，我们引入∧符号，把a写成一个矩阵，事实上是一个反对称矩阵（Skew-symmetric matrix），反对称矩阵A满足AT = -A。你可以将∧记成一个反对称符号。这样就把外积a × b写成了矩阵与向量的乘法a∧b，把它变成了线性运算。这意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然：

向量和加减法、内外积，即使在不谈论它们的坐标时也可以计算。例如，虽然内积在有坐标时，可以用两个向量的分量乘积之和表达，但是即使不知道它们的坐标时，也可以通过长度和夹角来计算二者的内积。所以两个向量的内积结果和坐标系的选取是无关的。

坐标系间的欧式变换

考虑运动的机器人，常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。相机或机器人则是一个移动坐标系。相机视野中某个向量p，它在相机坐标系下的坐标为pc，而在世界坐标系下看，它的坐标为pw。如果要实现这两个坐标之间的转换，需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中。我们需要一种数学手段来描述这个变换关系。

两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。相机运动就是一个刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化，只可能有空间位置和姿态的不同。此时，我们说刚体坐标系到世界坐标之间，相差了一个欧氏变换（Euclidean Transform）。

欧氏变换由旋转和平移组成。首先考虑旋转，设某个单位正交基 (e1, e2, e3) 经过一次旋转变成了 (e1’, e2’, e3’) ，那么，对于同一个向量 a（该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为[a1, a2, a3]T和[a1’, a2’, a3’]T。因为向量本身没变，根据坐标的定义，有：

为了描述两个坐标之间的关系，我们对上述等式的左右两边同时左乘

那么左边的系数就变成了单位矩阵，把中间的矩阵拿出来，定义成一个矩阵R：

这个矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。可以说，矩阵R描述了旋转本身。因此称为旋转矩阵（Rotation matrix）。该矩阵各分量是两个坐标系基的内积，由于基向量的长度为1，所以实际上是各基向量的夹角之余弦。所以这个矩阵也叫方向余弦矩阵（Direction Cosine matrix）。

旋转矩阵有一些特别的性质。它是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。以把 n 维旋转矩阵的集合定义如下：

SO(n) 是特殊正交群（Special Orthogonal Group）的意思。这个集合由n维空间的旋转矩阵组成，特别地，SO(3) 就是指三维空间的旋转。通过旋转矩阵，我们可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起。

由于旋转矩阵为正交矩阵，它的逆（即转置）描述了一个相反的旋转：

在欧氏变换中，除了旋转之外还有平移。考虑世界坐标系中的向量a，经过一次旋转（用R描述）和一次平移t后，得到了a′，把旋转和平移合到一起，有：

t称为平移向量。平移部分只需把平移向量加到旋转之后的坐标上。

通过上式，我们用一个旋转矩阵R和一个平移向量t完整地描述了一个欧氏空间的坐标变换关系。定义坐标系1、坐标系2，那么向量a在两个系下坐标为 a1, a2，它们之间的关系应该是：

R12 是指把坐标系2的向量变换到坐标系1中。同理，如果我们要表达从1到2的旋转矩阵时，就写成R21。关于平移t12，实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，把它记作从1到2的向量。但是反过来的t21，即从2指向1的向量在坐标系2下的坐标，并不等于−t12，这里和两个系的旋转还有关系。从向量层面来看，它们确实是反向的关系，但这两个向量的坐标值并不是相反数。

变换矩阵与齐次坐标

上面的变换关系并非一个线性关系，一旦变换多次就会非常繁杂。所以我们引入齐次坐标和变换矩阵：

这是一个数学技巧：我们在一个三维向量的末尾添加 1，将其变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成线性关系。该式中，矩阵T称为变换矩阵（Transform Matrix）。

我们暂时用ã表示a的齐次坐标。那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：

但是懒得区分齐次和非齐次的符号了，所以在不引起歧义的情况下，我们就直接把它写成b=Ta的样子，默认其中进行了齐次坐标的转换。要注意的是，不进行齐次坐标转换时，这边的乘法在矩阵维度上是不成立的。

变换矩阵T具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为0 向量，右下角为 1。这种矩阵又称为特殊欧氏群（Special Euclidean Group）：

与 SO(3) 一样，求解该矩阵的逆表示一个反向的变换：

同样，我们用T12 这样的写法来表示从2到1的变换。

总结：

首先介绍了向量及其坐标表示，并介绍了向量间的运算；然后，坐标系之间的运动由欧氏变换描述，它由平移和旋转组成。旋转可以由旋转矩阵 SO(3) 描述，而平移直接由一个R^3向量描述。最后，如果将平移和旋转放在一个矩阵中，就形成了变换矩阵 SE(3)。