Slam十四讲之第一讲和第二讲
目录
- 1. slam是什么？
- 2. 初始SLAM
- 3. 视觉SLAM框架
-
- 3.1 视觉里程计（VO）
- 3.2 后端优化（Optimization）
- 3.3 回环检测
- 3.4 建图
- 4. SLAM问题的数学表述
-
- 4.1 运动方程
- 4.2 观测方程
1. slam是什么？
SLAM 是 Simultaneous Localization and Mapping 的缩写，中文译作“同时定位与地图构建”。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动 。如果这里的传感器主要为相机，那就称为 “视觉 SLAM”。
本书主要分为两部分：
第一部分为数学基础篇
- 第一讲为前言
- 第二讲为 SLAM 系统概述，介绍一个 SLAM 系统由哪些模块组成，各模块的 具体工作是什么。实践部分介绍编程环境的搭建过程以及 IDE 的使用。
- 第三讲介绍三维空间运动，你将接触旋转矩阵、四元数、欧拉角的相关知识，并 且在 Eigen 当中使用它们。
- 第四讲为李群和李代数。如果你现在不懂李代数为何物，也没有关系。你将学 习李代数的定义和使用方式，然后通过 Sophus 操作它们。
- 第五讲介绍针孔相机模型以及图像在计算机中的表达。你将用 OpenCV 来调取 相机的内外参数。
- 第六讲介绍非线性优化，包括状态估计理论基础、最小二乘问题、梯度下降方法。你会完成一个使用 Ceres 和 g2o 进行曲线拟合的实验。
第二部分为SLAM技术篇
- 第七讲为特征点法的视觉里程计。该讲内容比较多，包括特征点的提取与匹配、 对极几何约束的计算、PnP 和 ICP 等。在实践中，你将用这些方法去估计两个 图像之间的运动。
- 第八讲为直接法的视觉里程计。你将学习光流和直接法的原理，然后利用 g2o 实现一个简单的 RGB-D 直接法。
- 第九讲为视觉里程计的实践章，你将搭建一个视觉里程计框架，综合应用先前 学过的知识，实现它的基本功能。从中你会碰到一些问题，例如优化的必要性、 关键帧的选择等。
- 第十讲为后端优化，主要为 Bundle Adjustment 的深入讨论，包括基本的 BA 以及如何利用稀疏性加速求解过程。你将用 Ceres 和 g2o 分别书写一个 BA 程序。
- 第十一讲主要讲后端优化中的位姿图。位姿图是表达关键帧之间约束的一种更 紧凑的形式。你将用 g2o 和 gtsam 对一个位姿球进行优化。
- 第十二讲为回环检测，我们主要介绍以词袋方法为主的回环检测。你将使用 dbow3 书写字典训练程序和回环检测程序。
- 第十三讲为地图构建。我们会讨论如何使用单目进行稠密深度图的估计（以及 这是多么不可靠），然后讨论 RGB-D 的稠密地图构建过程。你会书写极线搜索 与块匹配的程序，然后在 RGB-D 中遇到点云地图和八叉树地图的构建问题。
- 第十四讲主要介绍当前的开源 SLAM 项目以及未来的发展方向。相信阅读了前 面的知识之后，你会更容易理解它们的原理，实现自己的新想法。
2. 初始SLAM
自主运动两大基本问题：
我再什么地方？---------定位
周围长什么样子？-------- 建图
这是相互耦合的两个问题
准确的定位需要精确的地图
精确的地图来自准确的定位
定位和建图，可以看成感知的“内外之分”。一方面要明白自身的状态（即位置），另一方面也要了解外在的环境（即地图）。解决这两个问题的方法非常之多。比方说，我们可以在房间地板上铺设导引线，在墙壁上贴识别二维码，在桌子上放置无线电定位设备。如果在室外，还可以安装 GPS 定位设备。有了这些东西之后，定位问题是否已经解决了呢？我们不妨把这些传感器分为两类：
一类传感器是携带于机器人本体上的，例如机器人的轮式编码器、相机、激光等等
一类是安装于环境中的，例如前面讲的导轨、二维码标志等等。
相机：以一定速率采集图像，形成视频
分类：
单目 Monocular
双目 Stereo
深度 RGBD
其他 鱼眼 全景 Event
本质：以二维投影形式记录了三维世界的信息，此过程丢掉了一个纬度：距离
各类相机主要区别：有没有深度信息
单目：没有深度，必须通过移动相机，产生深度
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-z2VH5c9w-1686619391150)(E:\文档\图片\SLAM\1.PNG)]
双目：通过视差计算深度
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-j7eTgBW4-1686619391151)(E:\文档\图片\SLAM\2.PNG)]
RGBD：通过物理方法测量深度
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mGNrQym4-1686619391152)(E:\文档\图片\SLAM\3.PNG)]
3. 视觉SLAM框架
- 前端：VO
- 后端：Optimization
- 回环检测：Loop CLosing
- 建图：Mapping
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-8wb51i5f-1686619391152)(E:\文档\图片\SLAM\4.PNG)]
- 传感器信息读取：在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有硬盘，惯性传感器等信息的读取和同步
- **前端视觉里程计（VO）**视觉里程计的任务是估计相邻图像间相机的运动，以及局部地图的样子。VO又称前端
- 后端（非线性）优化（Optimization）：后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对他们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端。
- 回环检测（Loop Closure Detection）：回环检测判断机器人是否达到过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
- 建图：它根据估计的轨迹，建立与任务要求对应的地图
3.1 视觉里程计（VO）
视觉里程计关心相邻图像之间的相机运动，最简单的情况当然是两张图像之间的运动关系。VO 能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。 SLAM 叫它为里程计是因为它和实际的里程计一样，只计算相邻时刻的运动，而和再往前的 过去的信息没有关联，在这一点上，VO 就像一种只有很短时间记忆的物种一样。
现在，假定我们已有了一个视觉里程计，估计了两张图像间的相机运动。那么，只要 把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一 方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。 这么说来，有了 VO，是不是就解决了 SLAM 问题呢？
然而，仅通过视觉里程计来估计轨迹，将不可避免地出现累计漂移（Accumulating Drift）。这 是由于视觉里程计（在最简单的情况下）只估计两个图像间运动造成的。我们知道，每次 估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻， 导致经过一段时间之后，估计的轨迹将不再准确。比方说，机器人先向左转 90 度，再向右 转了 90 度。由于误差，我们把第一个 90 度估计成了 89 度。那我们就会尴尬地发现，向 右转之后机器人的估计位置并没有回到原点。更糟糕的是，即使之后的估计再准确，与真 实值相比，都会带上这-1 度的误差。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fLU75Ykf-1686619391153)(E:\文档\图片\SLAM\5.PNG)]
这也就是所谓的漂移（Drift）。它将导致我们无法建立一致的地图。你会发现原本直的 走廊变成了斜的，而原本 90 度的直角变成了歪的，为了解决漂移问题，我们还需要两种技术：后端优化和回环检测。回环检测负责把“机器 人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。
- 视觉里程计
- 通过相邻图像去估计相机运动
- 基本形式：通过两张图像计算运动和结构
- 不可避免地有漂移
- 方法
- 特征点法 第七讲
- 直接法 第八讲
3.2 后端优化（Optimization）
笼统地说，后端优化主要指处理 SLAM 过程中噪声的问题。传感器也带有一定的噪声，便宜的传感器测量误差较大，昂贵的则较小，有的传感器还会受磁场、温度的影响。
所以，除了解决“如何从图像估计出相机运动”之外，我们还要关心这个估计带有多大的噪声，这些噪声是如何从上 一时刻传递到下一时刻的、而我们又对当前的估计有多大的自信。
后端优化要考虑的问题， 就是如何从这些带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。这里的状态既包括机器人自身的轨迹，也包含地图。
在视觉 SLAM 中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
- 后端优化
- 从带有噪声的数据中优化轨迹和地图状态估计问题
- 最大后验概率估计MAP是一种参数估计方法，它基于贝叶斯定理，将先验概率和样本信息结合起来，得到后验概率，并选择使后验概率最大的参数作为估计值。在机器学习中，MAP通常用于解决分类和回归问题。
- 前期以EKF(滤波方法)为代表，现在以图优化为代表
3.3 回环检测
回环检测，又称闭环检测（Loop Closure Detection），主要解决位置估计随时间漂移的问题。怎么解决呢？假设实际情况下，机器人经过一段时间运动后回到了原点，但是由于漂移，它的位置估计值却没有回到原点。怎么办呢？我们想，如果有某种手段，让机器 人知道“回到了原点”这件事，或者把“原点”识别出来，我们再把位置估计值“拉”过 去，就可以消除漂移了。这就是所谓的回环检测。
回环检测与“定位”和“建图”二者都有密切的关系。事实上，我们认为，地图存在的主要意义，是为了让机器人知晓自己到达过的地方。为了实现回环检测，我们需要让机器人具有识别曾到达过的场景的能力。它的实现手段有很多。例如像前面说的那样，我们可以在机器人下方设置一个标志物（如一张二维码图片）。只要它看到了这个标志，就知道自己回到了原点。但是，该标志物实质上是一种环境中的传感器，对应用环境提出了限制（万一不能贴二维码怎么办呢？）。我们更希望机器人能使用携带的传感器——也就是图像本身，来完成这一任务。例如，我们可以判断图像间的相似性，来完成回环检测。这一点和人是相似的。当我们看到两张相似图片时，容易辨认它们来自同一个地方。如果回环检测成功，可以显著地减小累积误差。所以视觉回环检测，实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富，使得正确检测回环的难度也降低了不少。
在检测到回环之后，我们会把“A 与 B 是同一个点”这样的信息告诉后端优化算法。 然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如 果我们有充分而且正确的回环检测，就可以消除累积误差，得到全局一致的轨迹和地图。
- 回环检测
- 检测机器人是否回到早先位置
- 识别到达过的场景
- 计算图像间的相似性
- 方法：词袋模型
3.4 建图
建图（Mapping）是指构建地图的过程。地图是对环境的描述，但这个描述并不是固 定的，需要视 SLAM 的应用而定。对于地图，我们有太多的想法和需求。因此，相比于前面提到的视觉里程计、回环检测 和后端优化，建图并没有一个固定的形式和算法。一组空间点的集合也可以称为地图，一 个漂亮的 3D 模型亦是地图，一个标记着城市、村庄、铁路、河道的图片亦是地图。地图 的形式随 SLAM 的应用场合而定。大体上讲，它们可以分为度量地图与拓扑地图两种。
度量地图
度量地图强调精确地表示地图中物体的位置关系，通常我们用稀疏（Sparse）与稠密 （Dense）对它们进行分类。稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标（Landmark），那么一张稀疏地图 就是由路标组成的地图，而不是路标的部分就可以忽略掉。相对的，稠密地图着重于建模所有看到的东西。对于定位来说，稀疏路标地图就足够了。而用于导航时，我们往往需要稠密的地图（否则撞上两个路标之间的墙怎么办？）。
拓扑地图（Topological Map）
相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。拓扑地图是一个 图（Graph），由节点和边组成，只考虑节点间的连通性，例如 A，B 点是连通的，而不考 虑如何从 A 点到达 B 点的过程。它放松了地图对精确位置的需要，去掉地图的细节问题， 是一种更为紧凑的表达方式。然而，拓扑地图不擅长表达具有复杂结构的地图。如何对地图进行分割形成结点与边，又如何使用拓扑地图进行导航与路径规划，仍是有待研究的问 题。
4. SLAM问题的数学表述
假设机器人正携带着某种传感器在未知环境里运动，怎么用数学语言描述这件事呢？ 首先，由于相机通常是在某些时刻采集数据的，所以我们也只关心这些时刻的位置和地图。 这就把一段连续时间的运动变成了离散时刻 t = 1, . . . , K 当中发生的事情。在这些时刻， 用 x 表示机器人自身的位置。于是各时刻的位置就记为 x1, . . . , xK，它们构成了我们的运动轨迹。地图方面，我们设地图是由许多个路标（Landmark）组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 N 个，用 y1, . . . , yN 表示它们。
在这样设定中，“机器人携带着传感器在环境中运动”，由如下两件事情描述：
1.什么是运动？我们要考虑从 k − 1 时刻到 k 时刻，机器人的位置 x 是如何变化的。
2.什么是观测？假设机器人在 k 时刻，于 xk 处探测到了某一个路标 yj，我们要考虑这件事情是如何用数学语言来描述的。
4.1 运动方程
我们用一个一 般函数 f 来描述这个过程，而不具体指明 f 的作用方式。这使得整个函数可以指代任意 的运动传感器，成为一个通用的方程，而不必限定于某个特殊的传感器上。我们把它称为运动方程。
运动方程作用：通过上一时刻的位置，传感器的数据，误差来估计当前时刻的位置
- 离散时间： t = 1, . . . , K
- 机器人的位置：x1, . . . , xK
- 机器人从上一时刻运动到下一时刻
- 运动方程：xk = f (xk−1,uk, wk)
- xk-1 上一时刻的位置
- uk uk 是运动传感器的读数（有时也叫输入）
- wk 为噪声
4.2 观测方程
与运动方程相对应，还有一个**观测方程。**观测方程描述的是，当机器人在 xk 位置上 看到某个路标点 yj，产生了一个观测数据 zk,j。同样，我们用一个抽象的函数 h 来描述这 个关系
- 路标（三维空间点）： y1, . . . , yN
- 传感器在位置xk初，探测到了路标yj
- 观测方程：zk,j = h (yj , xk, vk,j )
- vk,j 是这次观测里的噪声