视觉 SLAM 十四讲 —— 第七讲视觉里程计1

前六讲介绍了 SLAM 会用到的基础知识，从这一讲将开始实际介绍 SLAM 的四个模块：视觉前端，优化后端，回环检测和地图构建。而视觉前端又被称为视觉里程计（Visual Odometry, VO），它根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。而根据是否需要提取特征，又分为特征点法和非特征点法。而由于基于特征点的前端运行稳定，对光照、动态物体不敏感，是当前 VO 的主流方法。

特征点法

特征点法通过提取图像中稳定、有特点的特征来描述图像，从而在更小的数据量上实现图像的描述。比较经典的人工特征包括 SIFT，SURF 和 ORB 等。

SIFT（尺度不变特征变换，Scale-Invariant Feature Transform）充分考虑了在图像变换过程中出现光照，尺度，旋转等变化，在所有人工特征点中性能可以说是最优的。但随之而来的是极大的计算量，而由于 SLAM 需要实时提取特征，SIFT 现阶段还无法满足实时性的要求，因此在 SLAM 中很好使用 SIFT 特征。
ORB 特征（Oriented FAST and Rotated BRIEF）通过改进 FAST 关键点使其具有了方向性，并采用二进制描述子 BRIEF 使得图像特征提取大大加速。

特征点提取速度对比（在同一副图像上提取约 1000 个特征点）：ORB 约要花费 15.3ms，SURF 约花费 217.3ms，SIFT 约花费 5228.7ms

由于 ORB 在质量和性能之间的良好平衡，现阶段 SLAM 主要都是基于 ORB 进行开发的。

ORB 特征

上面提到在 ORB 中实际使用到了 FAST 关键点提取方法和 BRIEF 描述子生成方法。下面分别介绍一下两个方法的操作流程。

FAST 关键点

FAST 关键点优化：

为了进一步提高速度，还可以通过直接检测 1,5,9,13 四个像素的亮度，只有这四个像素中有三个同事大于 $I_p+T$ 或小于时，才会进行后续的判断。
为解决 FAST 存在的特征点扎堆的情况，在第一遍检测后还会进行极大值抑制，在一定区域内仅保留响应极大值的角点。
为了能够指定提取的特征点数量，可以对原始 FAST 角点分别计算 Harris 响应值，然后选取前 N 个具有最大响应值的角点
使用图像金字塔解决不具备尺度不变性的问题
使用灰度质心法（选取几何中心到灰度质权重中心的向量作为特征点方向）提取方向信息

BRIEF 描述子

BRIEF 是一种二进制描述子，起选取关键点附近有顺序的两个像素，如果第一个大于第二个则取1，否则取0。如果选取了128对这样的像素对，那么 BRIEF 就是128维由0,1构成的向量。而关于像素对的选择有很多方法，BRIEF使用随机选点的方式，提高计算速度。

同时为了利用上面FAST提取的方向信息，这里对BRIEF进行改进，计算旋转之后的“steer BRIEF”，使得BRIEF具备了较好的旋转不变性

特征匹配是SLAM中极为关键的一步，其确定了当前看到的路标与之前看到的路标之间的对应关系。然而由于图像特征的局部特性，误匹配的情况广泛存在，目前已经成为SLAM中制约性能提升的一大瓶颈。

特征提取结束之后就是特征点的匹配，常用的有暴力匹配（Brute-Force Matcher，直接计算所有关键点之间的距离，然后选取距离最小的进行关联），快速近似最近邻（FLANN，通过构建检索结构加速匹配数据，适合特征点特别多的情况）。

2D-2D：对极几何

在获取了两张图像的对应关系之后，需要根据2D的对应关系，求解3D相机的运动情况，这个问题则通过对极几何来解决。

对极约束

根据第五讲中介绍的针孔相机模型，可得

其中 $\textbf{K}$ 是相机的内参矩阵， $\textbf{R},\textbf{t}$ 为两个坐标系的相机运动。

经过一系列等价变换，可得如下对极约束等式

上述等式中包含了平移和旋转，如果将中间部门抽象为基础矩阵 $\textbf{F}$ 和本质矩阵 $\textbf{E}$ ，可以进一步优化为

因此相机位姿估计问题变为先求解基础矩阵和本质矩阵，再结合相机内参得到旋转矩阵和平移向量。

本质矩阵

本质矩阵是一个 $3\times 3$ 的矩阵，但是满足如下特征：

由于对极约束等式为零，因此对本质矩阵乘以任意非零常数后，对极约束依然满足。这被称为本质矩阵在不同尺度下是等价的。
可以证明，本质矩阵的特征值满足 $[\sigma , \sigma, 0]^T$ 的形式，这被称为本质矩阵的内在性质。
本质矩阵实际上只有五个自由度。

不过由于本质矩阵的内在性质是一种非线性性质，因此如果只用五个点求解起来会非常困难。通常使用八点法（利用八个点对进行估计），因为只用到了线性性质，因此可以在线性代数框架下求解。

其中 e_1-e_9 是本质矩阵的9个元素， x_1^i=[u_1^i,v_1^i,1],x_2^i=[u_2^i,v_2^i,1] 为归一化之后的匹配点对。

求解上述线性方程可能存在四个解，不过好在四个解中只有一个在两个相机中都具有正的深度。因此只需要把任意一点代入四组解，只有都具有正深度的才是正确的解。

在求得E之后，对E进行SVD分解就可以得到两组旋转矩阵和平移向量

其中 $\textbf{R}_\textbf{z}(\frac{\pi}{2})$ 表示沿Z轴旋转90度得到的旋转矩阵。

最后由于直接求得的E可能不满足内在性质（特征值），因此在对E进行SVD分解（）后用下式求解。

这本质相当于将求出来的矩阵投影到了E所在的流形上。更简单的做法是由于E具有尺度等价性，可以将奇异值矩阵直接取成 diag(1,1,0) 。

单应矩阵

单应矩阵H描述的是两个平面之间的映射关系。比如场景中的特征点都落在同一个平面上（比如墙，地面等），则可以通过单应性来进行运动估计。

将特征点平面描述为

据此可以得到对应两个点的对应关系如下

这里，我们将中间的部分称为单应矩阵

其对应求解与本质矩阵类似，

单应性在SLAM中具有重要意义。当特征共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化。不过现实中的数据总包含一些噪声，这时候如果我们继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪
声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵和单应矩阵，选择重投影误差比较小的那个作为最终的运动估计矩阵。

讨论

尺度不确定性

单目SLAM基于两张图像无法获取尺度信息，因此E本身具有尺度等价性，从而求得到R,t也有一个尺度等价性。即如果求得的t第一维为0.822，此时无法确定单位是米还是厘米。因为对t乘以任意比例常数后，对极约束依然是成立的。

不过虽然我们不知道它的实际长度是多少，但我们以初始化的t的单位为1来计算相机运动和特性点的3D位置，这称为单目SLAM初始化。需要注意的是初始化两张图片必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。

初始化的纯旋转问题

单目初始化不能只有纯旋转，必须要有一定程度的平移。不过，如果相机只平移不旋转，就更容易让单目SLAM初始化。因此有经验的SLAM研究人员，在单目SLAM情况下，经常选择对相机进行左右平移以顺利地进行初始化。

多于八对点的情况

如果匹配对多于八个，那么可以将其转换一个最小二乘问题

不过由于噪声的存在，实际中更倾向于使用随机采样一致性（Random Sample Concensus, RANSAC）来求解。

三角测量

通过对极几何求得相机运动之后，由于单张图像无法获取像素的深度信息，因此单目SLAM中需要通过三角测量的方法来估计地图点的深度。

三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。

如果 x_1,x_2 为两个特征点的归一化坐标，那么它们满足

现在需要求解的是两个特征点的深度 s_1,s_2 ，由于R,t当前已知，因此可以根据下式直接求解。

当然，由于噪声的存在，更常见的做法是求最小二乘解而不是零解。

讨论

因此，要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，提高计算成本。另一方式是使平移量增大。但是，平移量增大，会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来了，比如反射光发生变化了，等等。外观变化会使得特征提取与匹配变得困难。总而言之，在增大平移，会导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。
如果假设特征点服从高斯分布，并且对它不断地进行观测，在信息正确的情况下，我们就能够期望它的方差会不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器（Depth Filter）。

3D-2D:PnP

前面介绍的2D-2D是需要初始化且需要八个或更多的点来进行相机运动估计。如果能通过双目或者RGB-D获取了一张特征点的3D信息，那么就可以使用更少的点（三个点，需要至少一个额外的点验证结果）不需要初始化直接估计相机运行，这就是3D-2D。具体实现有多重求解方法，下面依次介绍。

直接线性变换

假定某个空间点 $\textbf{P}=(X,Y,Z,1)^T$ ，基于旋转矩阵和平移向量构建增广矩阵 [R|t] ，可得如下等式

如果定义如下行向量

可以转换为如下线性方程组

方程组一共有12个自由度，至少需要六对特征点，多于六对同样可以使用 SVD 等方法对超定方程求最小二乘解。

同时，由于求解过程并没有限制旋转矩阵的特性，因此最后的结果需要进行调整：由QR分解把结果从矩阵空间重新投影到SE(3)流形上，转换成旋转和平移两部分。

最后这里默认内参矩阵K是已知的，如果未知，可以在求解中同步求解，不过由于变量的增加，求解的效果也会差一些。

P3P

P3P仅使用三对匹配点，对数据要求较少，有时也会采用。

由于已知ABC三点的世界坐标系信息和abc三点二维图像坐标信息，因此可以直接使用上述三角形相似关系得到ABC三点在相机坐标系下的位置。之后这个3D-2D问题就转变为3D-3D问题，后面将会介绍带匹配信息的3D-3D问题求解是相对容易的。

不过P3P也存在以下问题：

上述方法只能利用3个匹配点的信息，多于的匹配信息很难利用
如果数据中存在噪声，将使算法失效

Bundle Adjustment

在 SLAM当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整。这就是 Bundle Adjustment，它从非线性的角度解决 PnP 问题。

前面介绍的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。在 PnP 问题中，这个 Bundle Adjustment 问题，是一个最小化重投影误差（Reprojection error）的问题。

将相机位姿表示为李代数

不过由于相机位姿未知已经观测点的噪声，因此我们可以把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

误差基于相机位姿的导数为

误差基于空间点的导数为

3D-3D: ICP

最后我们讨论3D-3D的位姿估计问题，其基于已经匹配好的两组3D点计算相机位姿。其需要得到一个欧氏变换 R,t 使得两组三维点满足

可以看出3D-3D 位姿估计问题中，并没有出现相机模型，也就是说，仅考虑两组 3D 点之间的变换时，和相机并没有关系。因此，在激光 SLAM 中也会碰到 ICP（Iterative Closest Point，迭代最近点），不过由于激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。而在视觉中，特征点为我们提供了较好的匹配关系，所以整个问题就变得更简单了。

ICP 同样存在线性代数解法 SVD，以及利用非线性优化方式求解（类似于 Bundle Adjustment）。