视觉SLAM十四讲---预备知识

SLAM是什么？

SLAM是Simultaneous Localization and Mapping 的缩写，即同时定位与地图构建。
它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。如果这里的传感器主要为相机，那就称为”视觉SLAM“。

SLAM的目的

SLAM的目的是解决”定位“与”地图构建“问题，也就是说一边要估计传感器自身的位置，一边要建立周围环境的模型。
这需要用到传感器的信息。传感器以一定形式观察外部世界，但不同传感器观察的方式不同。
我们希望实时的、在没有先验知识的情况下进行SLAM。当用相机作为传感器时，我们要做的就是根据一张张连续运动的图像（它们形成了一段视频），从中推断相机的运动，以及周围环境的情况。

SLAM应用

室内的扫地机和移动机器人需要定位
野外的自动驾驶汽车需要定位
空中的无人机需要定位
虚拟现实和增强现实的设备。

SLAM系统模块

视觉里程计
后端优化
建图
回环检测

传感器分为两类

携带于机器人本体上的，例如机器人的轮式编码、相机、激光传感器等。
安装于环境中的，例如导轨、二维码标志等。安装于环境中的传感设备，通常能够直接测量机器人的位置信息，简单有效地解决定位问题。由于它们要求环境必须由人工步骤，在一定程度上限制了机器人的使用范围。

传感器约束了外部环境。只有在这些约束满足时，基于它们的定位方案才能工作。当约束无法满足时，我们就无法进行定位。这类传感器简单可靠，但他们无法提供一个普遍的、通用的解决方案。相对地，那些携带于机器人本体上的传感器，比如激光传感器、相机、轮式编码器、惯性测量单元（Inertial Measurement Unit, IMU)等，它们测量的通常都是一些间接的物理量而不是直接的位置数据。例如，轮式编码器会测量轮子转动的角度，IMU测量运动的角速度和加速度，相机和激光传感器则读取外部环境的某种观测数据。

使用携带式的传感器来完成SLAM是我们重点关心的问题。特别地，当谈论视觉SLAM时，我们主要时指如何用相机解决定位和建图问题。同样，如果传感器主要是激光，那就称为激光SLAM。

SLAM中使用地相机与我们平时见到的单反摄像头并不是同一个东西。它更加简单，通常不携带昂贵的镜头，而是以一定的速率拍摄周围的环境，形成一个连续的视频流。普通的摄像头能以每秒钟拍摄30张图片的速度采集图像，高速相机则更快一些。

按照工作方式的不同，相机可以分为单目（Monocular）相机、双目（Stereo）相机和深度（RGB-D）相机三大类。直观来看，单目相机只有一个摄像头，双目有两个，而RGB-D的原理较复杂，除了能够采集到彩色图片，还能读出每个像素与相机之间的距离。深度相机通常携带多个摄像头，工作原理和普通相机不尽相同。

SLAM中还有全景相机、Event相机等特殊或新兴的种类。

单目相机

只使用一个摄像头进行SLAM的做法称为单目SLAM（Monocular SLAM）。这种传感器结构特别简单，成本特别低，所以单目SLAM非常受研究者关注。
单目相机的数据：照片。照片本质上是拍摄某个场景（Scene）在相机的成像平面上留下的一个投影。它以二维的形式记录了三维的世界。这个过程丢掉了场景的一个维度，也就是所谓的深度（或距离）。在单目相机中，我们无法通过单张图片计算场景中物体与相机之间的距离（远近）。这个距离是SLAM中非常关键的信息。我们见过大量的图像，形成了一种天生的直觉，对大部分场景都有一个直观的距离感（空间感），它可以帮助我们判断图像中物体的远近关系。由于近大远小的透视关系，它们可能在图像中变成同样大的样子。
由于单目相机拍摄的图像只是三维空间的二维投影，所以，如果真想恢复三维结构，必须改变相机的视角。在单目SLAM中也是同样的原理。我们必须移动相机，才能估计它的运动（Motion），同时估计场景中物体的远近和大小，就是结构。近处的物体移动快，远处的物体移动慢，极远处（无穷远处）的物体（如太阳、月亮）看上去是不动的。于是，当相机移动时，这些物体在图像上的运动就形成了视差（Disparity）。通过视差，我们就能定量地判断哪些物体离得远，哪些物体离得近。即使我们知道了物体远近，它们仍然只是一个相对的值。
单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度（Scale）。由于单目SLAM无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性（Scale Ambiguity）。
平移之后才能计算深度，以及无法确定真实尺度，这两件事情给单目SLAM的应用造成了很大的麻烦。其根本原因是通过单张图像无法确定深度。为了得到深度，使用双目相机和深度相机。

双目相机和深度相机

使用双目相机和深度相机的目的是通过某种手段测量物体与相机之间的距离，克服单目相机无法知道距离的缺点。一旦知道了距离，场景的三维结构就可以通过单个图像恢复，同时消除尺度不确定性。
都是为了测量距离，但是双目相机与深度相机测量深度的原理是不一样的。双目相机由两个单目相机组成，但这两个相机之间的距离[称为基线（Baseline）]是已知的。我们通过这个基线来估计每个像素的空间位置-----这和人眼非常相似。
我们人类可以通过左右眼图像的差异判断物体的远近，在计算机上也是同样的道理。如果对双目相机进行拓展，则可以搭建多目相机，不过本质没有什么不同。
双目相机的数据：左眼图像，右眼图像。通过左右眼的差异，能够判断场景中物体与相机之间的距离。
双目相机测量到的深度范围与基线相关。基线距离越大，能够测量到的物体就越远，所以无人车上搭载的双目相机通常会是个很大的家伙。双目相机的距离估计是比较左右眼的图像获得的，并不依赖其他传感设备，所以它既可以应用在室内，也可以应用于室外。双目或多目相机的缺点是配置与标定均较为复杂，其深度量程和精度受双目的基线与分辨率所限，而且视差的计算非常消耗计算资源，需要使用GPU和FPGA设备加速，才能实时输出整张图像的距离信息。因此在现有的条件下，计算量是双目的主要问题之一。
深度相机（又称RGB-D相机）最大的特点是可以通过红外结构光或Time-og-Flight(ToF)原理，像激光传感器那样，通过主动向物体发射光并接收返回的光，测出物体与相机之间的距离。它并不像双目相机那样通过软件计算来解决，而是通过物理的测量手段，所以相比于双目相机可节省大量的计算资源。
目前常用的RGB-D相机包括Kinect/Kinect V2、Xtion Pro Live、RealSense等，在一些手机上人们也用它来识别人脸。现在多数RGB-D相机还存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量投射材质等诸多问题。在SLAM方面，主要用于室内，室外则比较难用。
RGB-D数据：深度相机可以直接测量物体的图像和距离，从而恢复三维结构。
相机在场景中运动，将会得到一系列连续变化的图像。视觉SLAM的目标，是通过这样的一些图像，进行定位和地图构建。只要我们输入数据，就可以往外不断地输出定位和地图信息。

经典视觉SLAM框架

整个视觉SLAM流程包括以下步骤：

**传感器信息读取。**在视觉SLAM中主要为相机图像信息地读取与预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息地读取和同步。
**前端视觉里程计（Visual Odometry，VO）。**视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端（Front End）。
**后端（非线性）优化（Optimization）。**后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端（Back End）。
**回环检测（Loop Closure Detection）。**回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
**建图（Mapping）。**它根据估计的轨迹，建立与任务要求对应的地图。
如果把工作环境限定在静态、刚体、光照变化不明显、没有人为干扰的场景，那么这种场景下的SLAM技术已经相当成熟。

视觉里程计

视觉里程计关心相邻图像之间的相机运动，最简单的情况当然是两张图像之间的运动关系。
在计算机视觉领域，人类在直觉上看来十分自然的事情，在计算机视觉中却非常困难。图像在计算机里只是一个数值矩阵。
在视觉SLAM中，我们只能看到一个个像素，知道它们是某些空间点在相机的成像平面上投影的结果。所以，为了定量地估计相机运动，必须先了解相机与空间点的几何关系。
视觉里程计能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。称它为”里程计“是因为它和实际的里程计一样，只计算相邻时刻的运动，而和过去的信息没有关联。在这一点上，视觉里程计就像一种只有短时记忆的物种（不过可以不限于两帧，数量可以更多一些，例如5-10帧）。
如果已经有一个视觉里程计，估计了两张图像间的相机运动。一方面，只要把相邻时刻的运动”串“起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。
视觉里程计确实是SLAM的关键，但是仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移（Accumulating Drift）。这是由于视觉里程计在最简单的情况下只估计两个图像间的运动造成的。
累积误差导致长时间估计不再准确，需要回环检测和全局校正。这也就是所谓的漂移（Drift）。它将导致我们无法建立一致的地图。
为了解决漂移问题，我们还需要两种技术：后端优化和回环检测。回环检测负责把”机器人回到原始位置“的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。

后端优化

后端优化主要是指处理SLAM过程中的噪声问题。在现实中，再精确的传感器也带有一定的噪声。便宜的传感器测量误差较大，昂贵的可能会小一点，有的传感器还会受磁场、温度的影响。除了解决”如何从图像估计出相机运动“，我们还要关心这个估计带有多大的噪声，这些噪声是如何从上一时刻传递到下一时刻的，而我们又对当前的估计又多大的自信。
后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大-----这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。这里的状态既包括机器人自身的轨迹，也包含地图。
视觉里程计部分有时被称为”前端“。在SLAM框架中，前端给后端提供待优化的数据，以及这些数据的初始值。而后端负责整体的优化过程，它往往面对的只有数据，不必关心这些数据到底来自什么传感器。
在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
从历史意义上说，现在我们称为后端优化的部分，在很长一段时间内直接被称为”SLAM研究“。早期的SLAM问题是一个状态估计问题----正是后端优化要解决的。在最早提出SLAM的一系列论文中，当时的人们称它为”空间状态不确定性的估计“（Spitial Uncertainty）.
SLAM的本质：对运动主体自身和周围环境空间不确定性的估计。为了解决SLAM问题，我们需要状态估计理论，把定位和建图的不确定性表达出来，然后采用滤波器或非线性优化，估计状态的均值和不确定性（方差）。

回环检测

回环检测，又称为闭环检测，主要解决位置估计随时间漂移的问题。
怎么解决呢？假设实际情况下机器人经过一段时间的运动后回到了原点，但是由于漂移，它的位置估计值却没有回到原点。怎么办呢？如果有某种手段，让机器人知道“回到了原点”这件事，或者把“原点”识别出来，我们再把位置估计值“拉”过去，就可以消除漂移了。这就是所谓的回环检测。
回环检测与“定位”和“建图”二者都有密切的关系。
事实上，我们认为，地图存在的主要意义是让机器人知晓自己到过的地方。为了实现回环检测，我们需要让机器人下方设置一个标志物（比如一张二维码图片）。它只要看到了这个标志，就知道了自己回到了原点。该标志物实质上是一种环境中的传感器，对应用环境做了限制。
我们更希望机器人能使用携带的传感器----也就是图像本身，来完成这一任务（例如，可以判断图像间的相似性来完成回环检测）。如果回环检测成功，则可以显著地减小累积误差。
视觉回环检测实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富，使得正确检测回环的难度降低了不少。
在检测到回环之后，我们会把”A和B是同一个点“这样的信息告诉后端优化算法。然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如果我们有充分且正确的回环检测，则可以消除累积误差，得到全局一致的轨迹和地图。

建图

建图是指构建地图的过程。地图是指构建地图的过程。地图是对环境的描述，但这个描述并不是固定的，需要视SLAM而定。
对于地图，我们有太多的想法和需求。因此，相比于前面提到的视觉里程计、后端优化和回环检测，建图并没有一个固定的形式和算法。一组空间点的集合可以称为地图，一个漂亮的3D模型也是地图，一个标记着城市、村庄、铁路、河道的图片还是地图。地图的形式随SLAM的应用场合而定。
大体上讲，可以分为度量地图与拓扑地图两种。

度量地图（Metric Map）

度量地图强调精确地表示地图中物体的位置关系，通常用**稀疏（Sparse）与稠密（Dense）*对其分类。
稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。
稠密地图着重于建模所有看到的东西。定位时用稀疏路标地图就足够了。而用于导航时，则往往需要稠密地图（否则撞上两个路标之间的墙怎么办？）稠密地图通常按照某种分辨率，由许多个小方块（Vovel）。通常，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。当查询某个空间位置时，地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法，如A，D*等，为机器人研究者所重视。但是我们也看到，一方面，这种地图需要存储每一个各点的状态，会耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。另一方面，大规模度量地图有时会出现一致性问题。很小的一点转向误差，可能会导致两间屋子的墙出现重叠，使地图无效。

拓扑地图（Topological Map）

相比于度量地图的精确性，拓扑地图更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性。（例如只关注A、B点是连通的，而不考虑如何从A点到达B点）。它放松了地图对精确位置的需要，去掉了地图的细节，是一种更为紧凑的表达方式。然而，拓扑地图不擅长表达具有复杂结构的地图。如何对地图进行分割，形成节点和边，又如何使用拓扑结构进行导航与路径规划，仍是有待研究的问题。

首先，要说明机器人位置x是什么。我们还未明确解释位置的意义。
在平面中运动，小萝卜可以用两个坐标加一个转角的形式将其位置参数化。
我们知道三维空间的运动由3个轴构成，所以小萝卜的运动要由3个轴上的平移，以及绕着3个轴的旋转来描述，一共有6个自由度。