视觉SLAM十四讲

内部交流，写的很乱，各路大神不建议观看以免影响思路

1、激光SLAM

激光SLAM相对成熟，比如2005年出版的《概率机器人》中就介绍了很多关于激光SLAM的知识，在ROS里也能找到许多关于激光定位、激光建图的现成软件。

2、单目SLAM

在相机移动时，近处的物体移动快，远处的物体移动慢，极远处的物体看上去是不动的。于是当相机移动时，这些物体在图像上的运动就形成了视差。通过视差，我们就能定量的判断哪些物体离得远，哪些物体离得近。

然而，即使我们知道了物体远近，它们仍然只是一个相对的值。单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度。由于单目SLAM无法仅凭图像确定这个真是尺度，所以又称为尺度不确定性。

3、双目相机与深度相机

3.1. 双目相机

计算机上的双目相机需要大量的计算才能（不太可靠的）估计每一个像素点的深度。双目相机探测的深度范围与基线相关，基线距离越大，能够测量到的物体就越远。双目相机的距离估计是比较左右眼的图像获得的，并不依赖其他传感设备，所以它即可应用在室内，又可以应用在室外。双目相机的缺点是配置和标定均较为复杂，其深度量程和精度受双目基线与分辨率所限，而且视差的计算非常消耗计算资源，需要使用GPU和FPGA设备加速。

4、经典视觉SLAM框架

前端视觉里程计：视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子

后端（非线性）优化：后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图

回环检测：回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。

建图：它根据估计的轨迹，建立与任务要求对应的地图。

因此，我们说，如果把工作环境限定在静态、刚体、光照变化不明显、没有人为干扰的场景，那么这种场景下的SLAM技术已经相当成熟。

4.1. 视觉里程计

视觉里程计可以通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。称它为“里程计”是因为它和实际的里程计一样，只计算相邻时刻的运动，而和过去的信息没有关联。在这一点上，视觉里程计就像一种只有短时记忆的物种（不过可以不限于两帧，数量可以更多一点，例如5~10帧）。

然而，仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移。这是因为视觉里程计在最简单的情况下只估计两个图像间的运动造成的，每次估计都会带有一定的误差，先前时刻的误差会传递到下一时刻，导致一段时间过后，估计的轨迹将不再准确。通过后端优化和回环检测来解决这个问题。回环检测判断是否回到原来位置，后端优化参考这个信息，校正整个轨迹的形状。

4.2. 后端优化

后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这成为最大后验概率估计。这里的状态既包括机器人自身的轨迹，也包含地图。在视觉SLAM中，前段和计算机视觉研究领域更为相关，比如图像特征提取与匹配等，后端则主要是滤波与非线性优化算法。

4.3 回环检测

假设实际情况下机器人经过一段时间的运动后回到了原点，但是由于漂移，它的位置估计值并没有回到原点。如果有某种手段能让机器人知道自己回到原点了，再把估计值拉回去，漂移就消除了。例如，可以判断图像间的相似性来完成回环检测。所以视觉回环检测实质上是一种计算图像数据相似性的算法。由于图像信息非常丰富，使得正确检测回环的难度降低了不少。

4.4 建图

度量地图强调精确地表示地图中物体的位置关系，有稀疏和稠密之分。稠密地图通常按照某种分辨率，由许多个小块组成，在二维度量地图中体现为许多个小格子，而在三维度量地图中则体现为许多小方块。通常，一个小块有占据、空闲、未知三种状态，以表达该格内是否有物体。当查询某个空间位置时，地图能够给出该位置是否可以通过的信息。

4.5 SLAM系统

按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。其中线性高斯系统（LG）是最简单的，它的无偏的最优估计可以由卡尔曼滤波器（KF）给出。而在复杂的非线性非高斯系统（None-Linear Non-Gaussian,NLNG系统）中，我们会使用以扩展卡尔曼滤波器（Extended Kalman Filter, EKF）和非线性优化两大类方法去求解。直至21世纪早期，以EKF为主的滤波器方法在SLAM中占据了主导地位。我们会在工作点处把系统线性化，并以预测——更新两大步骤进行求解。最早的视觉SLAM系统就是基于EKF开发的。随后，为了克服EKF的缺点（例如线性化误差和噪声高斯分布假设），人们开始使用例子滤波器（Particle Filter）等其他滤波器，乃至使用非线性优化的方法。是指今日，主流视觉SLAM使用以图优化为代表的优化技术进行状态估计。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法。

5、三维空间的刚体运动

6、第六讲非线性优化

现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。

非线性优化

状态估计问题，批量状态估计与最大后验估计

最小二乘的引出

非线性最小二乘

一阶和二阶梯度法

7、视觉里程计

1、理解图像特征点的意义，并掌握在单幅图像中提取特征点及多幅图像中匹配特征点的方法

2、理解对极几何的原理，利用对极几何的约束，恢复图像之间的摄像机的三维运动。

3、理解PNP问题，以及利用已知三维结构与图像的位置关系求解摄像机的三维运动。

4、理解ICP问题，以及利用点云的匹配关系求解摄像机的三维运动

5、何通过三角化获得二维图像上对应点的三维结构

7.1 特征点法

角点、边缘、像素区块中，角点的辨认是更容易的。角点提取算法有很多，例如Harris角点、FAST角点、GFTT角点，等等。它们大部分是2000年以前提出的算法。

然而，在大多数应用中，单纯的角点依然不能满足我们的很多需求。例如，从远处看上去是角点的地方，当相机离近之后，可能就不显示为角点了。或者，当旋转相机时，角点的外观会发生变化，我们也就不容易辨认出那是同一个角点了。为此，计算机视觉领域的研究者们在长年的研究中设计了许多更加稳定的局部图像特征，如注明的SIFT，SURF,ORB，等等。相比于朴素的角点，这些人工设计的特征点能够拥有如下特征：

（1）可重复性：相同的特征可以在不同的图像中找到

（2）可区别性：不同的特征有不同的表达

（3）高效率：同一图像中，特征点的数量应远小于像素的数量

（4）本地性（Locality）：特征仅与一小片图像区域相关

特征点由关键点和描述子两部分组成。例如，当我们说“在一张图像中计算SIFT特征点”时，是指“提取SIFT关键点，并计算SIFT描述子”两件事情。关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个向量，按照某种认为设计的方式，描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该由相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量空间上的距离相近，就可以认为它们是同样的特征点。

历史上，研究者们提出过许多图像特征。它们有些很精确，在相机的运动和光照变化下仍具有相似的表达，但相应地计算量较大。其中，SIFT（尺度不变特征变换）当属最为经典的一种。它充分考虑了在图像变换过程中出现的光照、尺度、旋转等变换，但随之而来的是极大的计算量。、

另一些特征，则考虑适当降低精度和鲁棒性，以提升计算的速度。例如，FAST关键点属于计算特别快的一种关键点（它没有描述子），而ORB（Oriented FAST and Rotated BRIEF）特征则是目前看来非常具有代表性的实时图像特征。它改进了FAST检测自不具有方向性的问题，并采用速度极快的二进制描述子BRIEF，使整个图像特征提取的环节大大加速。根据作者在论文中所述测试，在同一幅图像中同时提取约1000个关键点的情况下，ORB约花费15.3毫秒，SURF约花费217.3毫秒，SIFT约花费5228.7毫秒。由此可以看出，ORB在保持了特征子具有旋转、尺度不变性的同时，在速度方面提升明显，对于实时性要求很高的SLAM来说是个很好的选择。

7.2 ORB特征

ORB特征由关键点和描述子两部分组成。它的关键点称为“Oriented FAST"，是一种改进的FAST角点。它的描述子称为BRIEF。因此，提取ORB特征分为如下两个步骤：

1、FAST角点提取：找出图像中的“角点”。相较于原版的FAST，ORB中计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变特性

2、BRIEF描述子：对前一步提取出特征点的周围图像区域进行描述。ORB对BRIEF进行了一些改进，主要是指在BRIEF中使用了先前计算的方向信息。

FSAT关键点

FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思想是：如果一个像素与邻域的像素差别较大（过亮或者过暗），那么它更可能是角点。相比于其他角点检测算法，FAST只需比较像素亮度的大小，十分快捷。它的检测过程如下：

（1）在图像中选取像素p，假设它的亮度为 $I_{p}$

（2）设置一个阈值T（比如， $I_{p}$ 的20%）

（3）以像素p为中心，选取半径为3的圆上的16个像素点

（4）假如选取的圆上有连续的N个点的亮度大于 $I_{p}+T$ 或小于 $I_{p}-T$ ，那么像素p可以被认为是特征点（N通常取12，即FAST-12。其他常用的N取值为9和11，它们分别被称为FAST-9和FAST-11）

（5）循环以上四步，对每一个像素执行相同的操作

在FAST-12中，为了更高效，可以添加一项预测试操作，以快速排除绝大多数不是角点的像素。具体操作位，对于每个像素，直接检测邻域圆上的第1,5,9,13个像素的亮度。只有当这四个像素中有3个同时大于 $I_{p}+T$ 或小于 $I_{p}-T$ 时，当前像素才有可能是一个角点，否则应该直接排除。这样的预测试操作大大加速了角点检测。此外，原始的FAST角点经常出现“扎堆”的现象。所以在第一遍检测之后，还需要用非极大值抑制，在一定区域内仅保留响应极大值的角点，避免角点集中的问题。

FAST特征点的计算仅仅是比较像素间亮度的差异，所以速度非常快，但它也有重复性不强、分布不均匀的缺点。此外，FAST角点不具有方向信息。同时，由于它固定取半径为3的圆，存在尺度问题：远处看着像是角点的地方，接近后看可能就不是角点了。针对FAST角点不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法实现的。

金字塔是计算图视觉中常用的一种处理方法，金字塔底层是原始图像，没往上一层，就对图像进行一个固定倍率的缩放，这样我们就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中，我们可以匹配不同层上的图像，从而实现尺度不变性。例如，如果相机在后退，那么我们应该能够在上一个图像金字塔的上层和下一个图像金字塔的下层中找到匹配。

在旋转方面，我们计算特征点附近的图像灰度质心。所谓质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：

（1）在一个小的图像块B中，定义图像块的矩为

（2）通过矩可以找到图像块的质心：

（3）连接图像块的几何中心O与质心C，得到一个方向向量 $\vec{OC}$ ，于是特征点的方向可以定义为

通过以上方法，FAST角点便具有了尺度与旋转的描述，从而大大提升了其在不同图像之间表述的鲁棒性。所以在ORB中，把这种改进后的FAST称为Oriented FAST

BRIEF描述子

在提取Oriented FAST关键点后，我们对每个点计算其描述子。ORB使用改进的BRIEF特征描述。

BRIEF是一种二进制描述子，其描述向量由许多个0和1组成，这里的0和1编码了关键点附近两个随机像素（比如p和q）的大小关系：如果p比q大，则取1，反之就取0.如果我们取了128个这样的p,q，则最后得到128维由0,1组成的向量。BRIEF使用了随机选点的比较、速度非常快，而且由于使用了二进制表达，存储起来也十分方便，适用于实施的图像匹配。原始的BRIEF描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而ORB在FAST特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转之后的“Steer BRIEF”特征使ORB的描述子具有较好的旋转不变性。

由于考虑到了旋转和缩放，ORB在平移、旋转和缩放的变换下仍有良好的表现。同时，FAST和BRIEF的组合也非常高效，使得ORB特征在实时SLAM中非常受欢迎

7.3 特征匹配

特征匹配解决了SLAM中的数据关联问题，即确定当前看到的路标与之前看到的路标之间的对应关系。通过对图像与图像或者图像与地图之间的描述子进行准确匹配，我么你可以为后续的姿态估计、优化等操作减轻大量负担。然而，由于图像特征的局部特征，误匹配的情况广泛存在，而且长期以来一直没有得到有效解决、目前已经成为视觉SLAM中制约性能提升的一大瓶颈。部分原因是场景中存在大量的重复纹理，使得特征描述非常相似。在这种情况下，仅利用局部特征解决误匹配是非常困难的。

让我们先来看正确匹配的情况。考虑两个时刻的图像，如果在图像 $I_{t}$ 中提取到特征点 $x_{t}^{m}$ ,m = 1,2,...,M,在图像 $I_{t+1}$ 中提取到特征点 $x_{t+1}^{n}$ ，n = 1, 2, ..., N .如何寻找这两个集合元素的对应关系呢？最简单的特征匹配方法就是暴力匹配。即对每一个特征点 $x_{t}^{m}$ 与所有的 $x_{t+1}^{n}$ 测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。对于浮点类型的描述子，使用欧氏距离进行度量即可。而对于二进制的描述子（比如BRIEF这样的），我们往往使用汉明距离（Hamming distance）作为度量——两个二进制串之间的汉明距离，指的是其不同位数的个数

当特征点数量很大时，暴力匹配法的运算量将变得很大，特别是当想要匹配某个帧和一张地图的时候。此时，快速近似最近邻（FLANN）算法更加适合于匹配点数量极多的情况。由于这些匹配算法理论已经成熟，而且实现上也已集成到OpenCV，所以这里就不再描述它的技术细节了。

7.4 计算机相机运动

（1）当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何解决

（2）当相机为双目、RGB-D时，或者通过某种方法得到了距离信息，那么就是根据两组3D点估计运动。该问题通常用ICP解决

（3）如果一组为3D，一组为2D，即我们得到了一些3D点和它们在相机的投影位置，也能估计相机的运动。该问题通过PnP求解

7.5 2D-2D：对极几何

7.5.1 对极约束

7.5.2 本质矩阵

7.5.3 单应矩阵