《视觉SLAM14讲》学习小结——供小白快速入门ch7(上)

视觉里程计1--特征点法（上）

上一章分析了参数的优化方法，我们发现非线性参数优化问题需要猜测一个初始值。本章的研究就是为SLAM问题中的参数非线性优化提供一个初始值。许多初学者没能弄明白，为什么本章所介绍的对极几何，PnP和ICP等方法就可以定位相机和路标，还需要非线性优化（BA, Bundle Adjustment）。究其根本原因是二者求解中所采用的信息不同，所获得结果的精度不同。利用观测信息实时地预估摄像机的运动和确定图像点的三维坐标过程在视觉SLAM中称为视觉里程器。除了本篇基于特征点的方法外，还有下一篇基于直接法的视觉里程器。

一、特征点法

对一个景物在不同的位置和角度拍两张图，人眼可以很容易地辨别出两张图上哪两个点是属于景物中的同一个点。但对计算机而言，这是相当困难的。经过了多年的发展，人们终于找到了解决这个问题的方法，特征点法就是其中的一种。

为了简化起见，目前的特征点法都是针对灰色图片进行处理，因此需要先将彩色图片转为灰色图片。特征点就是能表征两张图片中表示同一点景物的点，它一般由关键点和描述子这两方面信息组成。关键点信息的目的在于定位特征点的位置，它一般选取角点，即图像中两条可观测直线的交叉点。描述子用于匹配关键点以确定它们是不是一对特征点。不同的关键点和描述子就组合成了不同的特征点类型或表述方法，这里主要介绍ORB特征。

1.FAST关键点

FAST（Features from Accelerated Segment Test）由Edward Rosten和Tom Drummond在2006年首先提出。其主要检测局部像素灰度变化明显的地方，以速度快而著称。其思想是：如果一个像素与领域的像素灰度差别比较大，那么它可能就是角点。具体算法如下：

（1）在图像中选取一个像素p，假设它的亮度为 I_p 。

（2）设置一个阈值T（比如 0.3I_p ）。

（3）以p为中心，选取半径为3 的圆上的16个像素点，如图1所示。

（4）如果圆上有连续N个点的亮度大于 I_p+T 或小于 I_p-T ，那么像素p可认为是特征点；N一般取9，11，12，分别称之为FAST-9，FAST-11，FAST-12特征点。

图1：FAST特征点

这样得到的关键点存在两个问题。其一是图像被放大或缩小以后，原来的关键点可能变成非关键点，更不用说特征点匹配了。解决这个问题的办法是图形金字塔，即得到一张图片后将其按不同的倍数放大和缩小得到一系列的图片，如图2所示。这样一个特征点同时和不同放大倍率下的图像比较，就可以避免尺度问题。

图2：使用金字塔可以匹配不同缩放倍率下的图像

第二个问题与后面要介绍的描述子有关。描述子的构建一般是基于目标固联坐标系（如世界坐标系）进行的。由于相机可能旋转，难以保证描述子在不同图像中的一致性。为了解决这个问题，可以引入图像的方向描述。其具体做法是在关键点周围取一个数据块，然后计算其体心（可以理解为关键点），接着按灰度加权的方法计算图像块的质心。连接体心和质心就得到一个方向向量 $\overrightarrow{OC}$ 。以后在进行描述子比对前，先把两个关键点的方向 $\overrightarrow{OC}$ 对齐。这样计算描述子就不会受到相机旋转的影响了。这样带方向的FAST关键点称之为Oriented FAST。

2.BRIEF描述子

BRIEF（Binary Robust Independent Elementary Features）描述子先在关键点周围按一定顺序取一定数目（比如128）的点对（如 p,q ）。如果 p>q ，则这个顺序点的取1，否则取0。这样我们就得到一串长度为128的二进制数据。很显然，不同关键点所对应的描述子往往是不同的，所以可以用描述子来描述关键点的特征。由于关键点添加了方向信息，使得BRIEF与其结合时可以保持方向的不变性。这种结合得到的特征点称为ORB(Oriented BRIEF)特征点。

3.特征点的匹配

在实际使用中，会得到许多特征点。但两张图片的特征点需要匹配时，最直接的方法是暴力匹配法。这种方法将第一张图片中的每一个特征点都与第二张图片中的所有点匹配，会产生很大计算量。特征点处理和匹配可以用现成库OpenCV来完成，我们暂时不需要了解其中的细节。在程序分析的篇章中，我们会进一步学习OpenCV的使用。

二、单目相机的定位

寻找特征点的目的是为了确定相机的位姿并得到路标的三维信息。下面具体讨论这个过程所采用的方法。如果是从两张2D照片的特征点匹配信息中恢复相机运动，可用对极几何理论。如果是根据两张3D特征点匹配信息中恢复相机运动，可用ICP理论解决。这种方法用于双目或带深度相机的情况。如果是从一组3D与一组2D特征点的匹配信息来确定相机运动，则可使用PnP技术。

1.2D-2D:对极几何

问题描述：已知两帧图片中特征点坐标的对应关系，确定这两张图片所对应相机位置的位姿关系 $\mathbf{R},\mathbf{t}$ 。

(1)对极约束

在进一步分析之前，我们先来回忆ch5中所规定的几个坐标和标记。用 $\mathbf{P}=[X,Y,Z]^\mathrm{T}$ 表示空间点在相机坐标系中的三维坐标，用 $\mathbf{P}^w=[X^w,Y^w,Z^w]^\mathrm{T}$ 表示空间点在世界坐标系中的坐标，用 $\mathbf{p}^\prime=[x^\prime,y^\prime]^\mathrm{T}$ 表示像平面坐标，用 $\mathbf{p}^1=[x^1,y^1]^\mathrm{T}$ 表示归一化平面坐标，用 $\mathbf{p}=[u,v]^\mathrm{T}$ 表示像素平面坐标。即所有的三维坐标用大写，所有的二维坐标用小写，坐标标记在右上方，这是为了方便后面用不同的下标区分特征点。三维相机坐标和二维像素平面坐标无标记，世界坐标系用w，像平面坐标用" $\prime$ "，归一化坐标系用1标记。

现在假设我们已经得到了一对配对好特征点对的2D图片，则可根据图3所示的对极几何关系来确定这两张照片拍摄时相机的位姿差异或变化。

图3：对极几何约束

其中P点为三维空间中物理目标的位置， O_1,O_2 分别为两个相机的光心位置。这三个点所构成的平面称为极平面。极平面与两个相机的（虚拟）像平面的交线分别为极线 l_1,l_2 。 $\mathbf{p}_1^\prime,\mathbf{p}_2^\prime$ 分别为像点，极点 e_1,e_2 是光心连线即基线 O_1O_2 与两个像平面的交点。

根据世界坐标与相机坐标的关系有 $\mathbf{P}=\mathbf{R}\mathbf{P}^w+\mathbf{t}$ 。不失一般性，可设在第一帧时 $\mathbf{P}_1=\mathbf{P}^w$ ，其中 $\mathbf{P}_1$ 的下标1表示第一帧。这样 $\mathbf{R},\mathbf{t}$ 实际上表征相机在第二帧时刻相对第一帧的位姿。根据这些定义以及ch5中的式（3）有

$Z_1\mathbf{p}_1=\mathbf{K}\mathbf{P}_1,~Z_2\mathbf{p}_2=\mathbf{K}(\mathbf{R}\mathbf{P}^w+\mathbf{t}) =\mathbf{K}(\mathbf{R}\mathbf{P}_1+\mathbf{t})$ (1)

其中下标1，2分别是两帧图片的标记。根据ch5中的式（4）有

$\mathbf{p}_1=\mathbf{K}\mathbf{p}_1^1,~\mathbf{p}_2=\mathbf{K}\mathbf{p}_2^1$ (2)

合并以上两式可得

$Z_2 \mathbf{p}^1_2=Z_1\mathbf{R}\mathbf{p}^1_1+\mathbf{t}$ (3)

将这个结果两边同时左乘 $\mathbf{t}^\wedge$ 可消去等号右端的 $\mathbf{t}$ 项，再左乘 $(\mathbf{p}^1_2)^\mathrm{T}$ 可得 $Z_2(\mathbf{p}_2^1)^\mathrm{T}\mathbf{t}^\wedge \mathbf{p}^1_2=Z^1(\mathbf{p}_2^1)^\mathrm{T}\mathbf{t}^\wedge\mathbf{R} \mathbf{p}^1_1$ 。根据混合积公式可知此式左边为零，于是 $(\mathbf{p}_2^1)^\mathrm{T}\mathbf{t}^\wedge\mathbf{R}\mathbf{p}^1_1=0$ 。再次利用关系式（2）消去 $\mathbf{p}^1_1$ 和 $\mathbf{p}^1_2$ 可得

$(\mathbf{p}_2)^\mathrm{T}\mathbf{K}^\mathrm{-T}\mathbf{t}^\wedge\mathbf{R} \mathbf{K}^\mathrm{-1}\mathbf{p}_1=0$ (4)

这个关系式称为对极约束。在已知内参的情况下，它建立了两帧像素坐标与相机位姿变化的关系。 $\mathbf{E}=\mathbf{t}^\wedge\mathbf{R}$ 表示相机的位姿变化信息，称为基础矩阵。 $\mathbf{F}=\mathbf{K}^{-T}\mathbf{E}\mathbf{K}^{-1}$ 进一步考虑了内参信息，称为本质矩阵。这时上式可改写为

$(\mathbf{p}_2^1)^\mathrm{T}\mathbf{E} \mathbf{p}^1_1=\mathbf{p}_2^\mathrm{T}\mathbf{F}\mathbf{p}_1=0$ (5)

研究对极几何的目的是为了根据 $\mathbf{p}_1,~\mathbf{p}_2$ 信息确定 $\mathbf{t},\mathbf{R}$ 。

(2) $\mathbf{t},\mathbf{R}$ 的确定方法

确定 $\mathbf{t},\mathbf{R}$ 以满足 $\mathbf{E}=\mathbf{t}^\wedge\mathbf{R}$ 的方法很多，这里仅介绍常用的八点法。将式（5）的第一项中各元素明确后可得

$\begin{bmatrix} x_2^1 &y_2^1 &1 \end{bmatrix} \begin{bmatrix} e_1 & e_2 &e_3 \\ e_4&e_5 &e_6 \\ e_7 &e_8 & e_9 \end{bmatrix}\begin{bmatrix} x_1^1 \\y_1^1 \\1 \end{bmatrix}=0$ (6)

将其展开可得如下矩阵形式的线性方程

$[x_2^1x_1^1,x_2^1y_1^1,x_2^1,y_2^1x_1^1,y_2^1y_1^1,y_2^1,x_1^1,y_1^1,1]\mathbf{e}=0$ (7)

其中 $\mathbf{e}=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9]^\mathrm{T}$ 。取八对特征点即可构成一个8方程9未知数的方程组。不失一般性可令其中的 e_9=1 ，这样可使方程组封闭并求解其他 $\mathbf{e}$ 中的元素，即确定基础矩阵 $\mathbf{E}$ 。之所以可以强制 $\mathbf{e}$ 中的某个元素为零，是因为 $\mathbf{E}$ 矩阵具有尺度等价性。也就是说， $\mathbf{E}$ 中各元素同时放大一个非零的倍数，方程（5）仍然成立。

下面讨论已知 $\mathbf{E}$ 情况下计算 $\mathbf{t},\mathbf{R}$ 的方法。首先将 $\mathbf{E}$ 进行SVD分解为 $\mathbf{E}=\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^\mathrm{T}$ ，其中 $\mathbf{U},\mathbf{V}$ 为正交阵， $\boldsymbol{\Sigma}=\mathrm{diag}(\sigma_1,\sigma_2,\sigma_3)$ 为奇异值矩阵。因为尺度等价性，最小奇异值 $\sigma_3$ 理论上应当为零。由于数字计算误差 $\sigma_3$ 一般不为零，这时需要将 $\mathbf{E}$ 投影到其流形上，即令

$\mathbf{E}=\mathbf{U}\boldsymbol{\Sigma}^\prime\mathbf{V}^\mathrm{T}=\mathbf{U}\mathrm{diag}(\frac{\sigma_1+\sigma_3}{2},\frac{\sigma_2+\sigma_3}{2},0)\mathbf{V}^\mathrm{T}$ (8)

这时可以发现如下两个正交矩阵 $\mathbf{t}^\wedge,\mathbf{R}$ 满足关系式 $\mathbf{E}=\mathbf{t}^\wedge\mathbf{R}$

$\left\{\begin{matrix} \mathbf{t}_1^\wedge=\mathbf{U}\mathbf{R}_z(\frac{\pi}{2})\boldsymbol{\Sigma}^\prime\mathbf{U}^\mathrm{T},~ \mathbf{R}_1=\mathbf{U}\mathbf{R}_z(\frac{\pi}{2})\mathbf{V}^\mathrm{T}~~~~~\\ \mathbf{t}_2^\wedge=\mathbf{U}\mathbf{R}_z(-\frac{\pi}{2})\boldsymbol{\Sigma}^\prime\mathbf{U}^\mathrm{T},~ \mathbf{R}_2=\mathbf{U}\mathbf{R}_z(-\frac{\pi}{2})\mathbf{V}^\mathrm{T} \end{matrix}\right.$ (9)

其中 $\mathbf{R}_z(\frac{\pi}{2})$ 表示绕 $\mathbf{z}$ 逆时针旋转 ${\pi}/{2}$ 的旋转矩阵。根据ch3中的讨论，它等于 $-\mathbf{C}_3(\frac{\pi}{2})$ 。如果读者没有完全理解ch3第四节的内容，误以为应该是 $\mathbf{C}_3(\frac{\pi}{2})$ ，并不会对这里的讨论结果产生影响。这是因为根据式（5）可知， $-\mathbf{E}$ 也是此方程的解。这样将 $\mathbf{E}$ 用 $-\mathbf{E}$ 代替并重复以上步骤可以得到另外两组 $\mathbf{t}^\wedge,\mathbf{R}$ 。无论是采用 $\mathbf{C}_3(\frac{\pi}{2})$ 还是 $-\mathbf{C}_3(\frac{\pi}{2})$ ，这四组 $\mathbf{t}^\wedge,\mathbf{R}$ 解是一样的，并构成了 $\mathbf{t},\mathbf{R}$ 的四个可能解。它们所对应的相机位姿分别如图4所示，其中只有第一种结果是正确的。为了确定哪组 $\mathbf{t},\mathbf{R}$ 为正确值，可在保证 Z>0 的情况下取一个 $\mathbf{P}_1=[X,Y,Z]^\mathrm{T}$ ，接着根据计算的到的 $\mathbf{t}^\wedge,\mathbf{R}$ 按式（1）计算 $Z_1\mathbf{p}_1$ 和 $Z_2\mathbf{p}_2$ 。由于 $\mathbf{p}_1$ 和 $\mathbf{p}_2$ 中的元素均为正，因此根据这个结果可确定 Z_1,Z_2 的符号。只有它们均为正的那一组 $\mathbf{t}^\wedge,\mathbf{R}$ 才是正确值。