《视觉SLAM十四讲》学习笔记（一）
第二讲 初识SLAM（一）
SLAM— Simultaneous Localization and Mapping 同时定位与地图构建
2.1 引子及简单介绍
它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过****程中建立环境的模型，同时估计自己的运动
即使我们知道了物体远近，它们仍然只是一个相对的值。想象我们在看电影时候，虽然能够知道电影场景中哪些物体比另一些大，但我们无法确定电影里那些物体的“真实尺度”：那些大楼是真实的高楼大厦，还是放在桌上的模型？而摧毁大厦的是真实怪兽，还是穿着特摄服装的演员？直观地说，如果把相机的运动和场景大小同时放大两倍，单目所看到的像是一样的。同样的，把这个大小乘以任意倍数，我们都将看到一样的景象。这说明了单目SLAM 估计的轨迹和地图，将与真实的轨迹、地图，相差一个因子，也就是所谓的尺度（Scale）¬。由于单目SLAM 无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性。
平移之后才能计算深度，以及无法确定真实尺度，这两件事情给单目SLAM 的应用造成了很大的麻烦。它们的本质原因是通过单张图像无法确定深度。所以，为了得到这个深度，人们又开始使用双目和深度相机。
双目相机测量到的深度范围与基线相关。基线距离越大，能够测量到的就越远。计算量是双目的主要问题之一。
深度相机（又称RGB-D 相机，在本书中主要使用RGB-D 这个名称）是2010 年左右开始兴起的一种相机，它最大的特点是可以通过红外结构光或Time-of-Flight（ToF）原理，像激光传感器那样，通过主动向物体发射光并接收返回的光，测出物体离相机的距离。通过物理的测量手段，所以相比于双目可节省大量的计算量。
存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题。
2.2 经典视觉SLAM框架
经典视觉SLAM框架
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-pPCx3lDn-1571909505304)(C:\Users\Administrator\AppData\Roaming\Typora\typora-user-images\image-20191024121648670.png)]
视觉SLAM流程：
- 传感器信息读取。在视觉SLAM 中主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
- 视觉里程计(Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。VO 又称为前端（Front End）。
- 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO 之后，又称为后端（Back End）。
- 回环检测（Loop Closing）。回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
- 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。
2.2.1 视觉里程计
VO：叫它为“里程计”是因为它和实际的里程计一样，只计算相邻时刻的运动，而和再往前的过去的信息没有关联。在这一点上，VO 就像一种只有很短时间记忆的物种一样。假定我们已有了一个视觉里程计，估计了两张图像间的相机运动。那么，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。
-
问题：仅通过视觉里程计来估计轨迹，将不可避免地出现累计漂移（Accumulating Drift）。
-
**why:**每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻。
-
**结果:**将导致我们无法建立一致的地图。
-
解决办法：为了解决漂移问题，我们还需要两种技术：后端优化（更多时候称为后端(Back End)。由于主要使用的是优化方法，故称为后端优化）和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。
2.2.2 后端优化
笼统地说，后端优化主要指处理SLAM 过程中噪声的问题。
除了解决“如何从图像估计出相机运动”之外，我们还要关心这个估计带有多大的噪声，这些噪声是如何从上一时刻传递到下一时刻的、而我们又对当前的估计有多大的自信。
后端优化解决的问题：后端优化要考虑的问题，就是如何从这些带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。这里的状态既包括机器人自身的轨迹，也包含地图。
数据来源：
在视觉SLAM 中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
2.2.3 回环检测
回环检测，又称闭环检测（Loop Closure Detection），主要解决位置估计随时间漂移的问题。
问题：机器人经过一段时间运动后回到了原点，但是由于漂移，它的位置估计值却没有回到原点，How to do?
**Idea:**如果有某种手段，让机器人知道“回到了原点”这件事，或者把“原点”识别出来，我们再把位置估计值“拉”过去，就可以消除漂移了。这就是所谓的回环检测。
解决办法：。我们更希望机器人能使用携带的传感器——也就是图像本身，来完成这一任务。例如，我们可以判断图像间的相似性，来完成回环检测。这一点和人是相似的。当我们看到两张相似图片时，容易辨认它们来自同一个地方。如果回环检测成功，可以显著地减小累积误差。所以视觉回环检测，实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富，使得正确检测回环的难度也降低了不少。在检测到回环之后，我们会把“A 与B 是同一个点”这样的信息告诉后端优化算法。然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如果我们有充分而且正确的回环检测，就可以消除累积误差，得到全局一致的轨迹和地图。
2.2.4 建图
**定义：**建图（Mapping）是指构建地图的过程。地图是对环境的描述，但这个描述并不是固定的，需要视SLAM 的应用而定。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zEUv4jwc-1571909505306)(C:\Users\Administrator\AppData\Roaming\Typora\typora-user-images\image-20191024163913906.png)]
地图：形式随SLAM 的应用场合而定。大体上讲，它们可以分为度量地图与拓扑地图两种。
-
度量地图（Metric Map）
度量地图强调精确地表示地图中物体的位置关系，通常我们用**稀疏（Sparse）与稠密（Dense）**对它们进行分类。
稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标（Landmark），那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略掉。
稠密地图着重于建模所有看到的东西。对于定位来说，稀疏路标地图就足够了。而用于导航时，我们往往需要稠密的地图**（否则撞上两个路标之间的墙怎么办？）**。稠密地图通常按照某种分辨率，由许多个小块组成。
- 二维度量地图是许多个小格子（Grid），三维则是许多小方块（Voxel）。一般地，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。当我们查询某个空间位置时，地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法，如A*,D*等等，为机器人研究者们所重视。
- **问题：**这种地图需要存储每一个格点的状态，耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。另一方面，大规模度量地图有时会出现一致性问题。很小的一点转向误差，可能会导致两间屋子的墙出现重叠，使得地图失效。
-
拓扑地图（Topological Map）
相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性，例如A，B 点是连通的，而不考虑如何从A 点到达B 点的过程。它放松了地图对精确位置的需要，去掉地图的细节问题，是一种更为紧凑的表达方式。
**缺点：**拓扑地图不擅长表达具有复杂结构的地图。
2.3 SLAM问题的数学表述
场景应用设定和数学规定：
- 假设小萝卜正携带着某种传感器在未知环境里运动，怎么用数学语言描述这件事呢？首先，由于相机通常是在某些时刻采集数据的，所以我们也只关心这些时刻的位置和地图。这就把一段连续时间的运动变成了离散时刻 t = 1 , … , K t=1,…,K t=1,…,K 当中发生的事情。在这些时刻，用 x x x 表示小萝卜自身的位置。于是各时刻的位置就记为 x 1 , . . . x K x_1,...x_K x1,...xK，它们构成了小萝卜的轨迹。地图方面，我们设地图是由许多个路标（Landmark）组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 N N N 个，用 y 1 , . . . , y N y_1,...,y_N y1,...,yN表示它们。
在这样设定中，“小萝卜携带着传感器在环境中运动”，由如下两件事情描述：
- 什么是运动？我们要考虑从 k − 1 k-1 k−1时刻到 k k k时刻，小萝卜的位置 x x x是如何变化的。
- 什么是观测？假设小萝卜在 k k k 时刻，于 x k x_k xk处探测到了某一个路标 y i y_i yi，我们要考虑这件事情是如何用数学语言来描述的。
x k = f ( x k − 1 , u k , w k ) . (2.1) x_k=f(x_{k-1},u_k,w_k). \tag{2.1} xk=f(xk−1,uk,wk).(2.1)
这里 u k u_k uk是运动传感器的读数（有时也叫输入）， w k w_k wk为噪声。我们把 ( 2.1 ) (2.1) (2.1)称为运动方程。
与运动方程相对应，还有一个观测方程。观测方程描述的是，当小萝卜在 x k x_k xk位置上看到某个路标点 y j y_j yj，产生了一个观测数据 z k , j z_{k,j} zk,j。同样，我们用一个抽象的函数 h h h来描述这个关系：
z k , j = h ( y j , x k , v k , j ) . (2.2) z_{k,j}=h(y_j,x_k,v_{k,j}). \tag{2.2} zk,j=h(yj,xk,vk,j).(2.2)
这里 v k , j v_{k,j} vk,j是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据 z z z以及观测方程 h h h也许多不同的形式。