引子：小萝卜的例子
希望小萝卜具有自主运动能力。
定位和建图可以看成感知的内外之分：
- 一方面，要明白自身的状态（即位置）；
- 另一方面，也要了解外在的环境（即地图）。
一类传感器是携带于机器人本体上的，测到的通常都是一些间接的物理量而不是直接的位置数据，这种定位方案可适用于未知环境；
另一类是安装于环境中的，限制了机器人的使用范围。
当谈论视觉 SLAM时，主要是指如何用相机解决定位和建图问题。
相机：以一定速率拍摄周围的环境，形成一个连续的视频流。
按照工作方式的不同，可分为：单目相机（Monocular）、双目相机（Stereo）、深度相机（RGB-D）。
单目相机
照片本质上是拍照时的场景（Scene）在相机的成像平面上留下的一个投影。它以二维的形式反映了三维的世界。
运动（Motion）：如果相机往右移动，那么图像里的东西就会往左边移动，这就给我们推测运动带来了信息；
结构（Structure）：近处的物体移动快，远处的物体则运动缓慢。于是，在相机移动时，这些物体在图像上的运动就形成了视差。
单目 SLAM 估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度（Scale）。
平移之后才能计算深度，以及无法确定真实尺度。根本原因是：通过单张图像无法确定深度。
双目相机
两个相机之间的距离（基线：Baseline）是已知的，这和人眼非常相似。
基线距离越大，能够测量到的就越远。
相对于人类非常笨拙，需要大量的计算才能估计每一个像素点的深度。需要使用 GPU 和 FPGA 设备加速后，才能实时输出整张图像的距离信息，计算量是双目的主要问题之一。
深度相机
通过红外结构光或 Time-of-Flight（ToF）原理，主动向物体发射光并接收返回的光。
不像双目相机那样通过软件计算来解决。
主要用于室内，室外则较难应用。
经典视觉 SLAM 框架
SLAM 需要一个完善的框架：
如果把工作环境限定在静态、刚体、光照变化不明显、没有人为干扰的场景，那么，这个 SLAM 系统是相当成熟的了。
视觉里程计（Visual Odometry）
VO 又称为前端（Front End），只计算相邻时刻的运动，而和再往前的过去的信息没有关联。
仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移（Accumulating Drift），解决漂移问题，需要两种技术：
- 回环检测：负责把“机器人回到原始位置”的事情检测出来；
- 后端优化：根据该信息，校正整个轨迹的形状。
后端优化（Optimization）
后端接收不同时刻视觉里程计测量的相机位姿，以及回环检测的信息。由于接在 VO 之后，又称为后端（Back End）。
后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。
前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等；后端则主要是滤波与非线性优化算法。
SLAM 问题的本质：对运动主体自身和周围环境空间不确定性的估计。
回环检测（Loop Closing）
如果检测到回环，它会把信息提供给后端进行处理。
需要让机器人具有识别到过的场景的能力，可以判断图像间的相似性来完成回环检测。
由于图像的信息非常丰富，使得正确检测回环的难度降低了不少。
建图（Mapping）
一个相机，它有 6 个自由度的运动，我们至少需要一张三维的地图；想要一个漂亮的重建结果，不仅是一组空间点，还需要带纹理的三角面片。
对于地图，大体上可以分为度量地图和拓扑地图两种。
度量地图（Metric Map）
- 稀疏（Sparse）地图：由路标（Landmark）组成的地图，而不是路标的部分就可以忽略掉，对于
定位来说，稀疏地图就足够了； - 稠密（Dense）地图：着重于建模所有看到的东西，用于
导航时，往往需要稠密的地图。
二维度量地图由许多小格子（Grid）组成，三维度量地图则由许多小方块（Voxel）组成。一个小块含有占据、空闲、未知三种状态。
需要存储每一个格点的状态，会耗费大量的存储空间，许多细节部分是无用的；有时候会出现一致性问题，很小的一点转向误差，可能会导致出现重叠，使地图失效。
拓扑地图（Topological Map）
由节点和边组成，只考虑节点间的连通性，不考虑如何从 A 点到达 B 点。是一种更为紧凑的表达方式。
SLAM 问题的数学表述
- 运动：我们要考虑从 k-1 时刻到 k 时刻，小萝卜的位置 x 是如何变化的；
- 观测：假设小萝卜在 k 时刻于 xk处探测到了某一路标yj，我们要考虑这件事情是如何用数学语言来描述的。
针对不同的传感器，运动方程和观测方程有不同的参数化形式，我们把它们取成通用的抽象形式：
这两个方程描述了最基本的 SLAM 问题：当知道运动测量的读数 u，以及传感器的读数 z 时，如何求解定位问题（估计 x）和建图问题（估计 y）？这时，我们就把 SLAM 问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？
按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类。分为线性/非线性和高斯/非高斯系统。
其中，线性高斯系统（Linear Gaussian，LG）是最简单的，它的无偏的最优估计可以由卡尔曼滤波器（Kalman Filter，KF）给出；而在复杂的非线形非高斯系统（Non-Linear Non-Gaussian，NLNG）中，我们会使用以扩展卡尔曼滤波器（Extended Kalman Filter，EKF）和非线性优化两大类方法去求解。
时至今日，主流视觉 SLAM 使用以图优化（Graph Optimization）为代表的优化技术进行状态估计。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法。
数学知识：
- 对 6 自由度的位姿，如何表达它，如何优化它；
- 观测方程如何参数化，空间中的路标点是如何投影到一张照片上。需要解释相机的成像模型；
- 知道了这些信息，怎么求解上述方程。需要非线性优化的知识。
位姿这个词表示“位置”加上“姿态”，包含了旋转（Rotation）和平移（Translation）。
实践：编程基础
使用 cmake
任意一个 C++ 程序都可以用 g++ 来编译，在历史上工程师们曾使用 makefile 进行自动编译，cmake 比它更加方便。
在一个 cmake 工程中，我们会用 cmake 命令生成一个 makefile 文件，然后，用 make 命令根据这个 makefile 文件的内容编译整个工程。
MakeFile 是一个自动化编译的脚本，我们用 cmake-make 的做法，cmake 过程处理了工程文件之间的关系，降低了维护整个工程的难度；而 make 过程实际调用了 g++ 来编译程序。
我们新建了一个中间文件夹“build”，这样，cmake 产生的中间文件就会生成在 build 文件夹中，当发布源代码时，只要把 build 文件夹删掉即可。
使用库
程序代码由头文件和源文件组成，带有 main 函数的源文件编译成可执行程序，其他的编译成库文件。
一个库往往是许多算法、程序的集合。
在 Linux 中，库文件分成静态库和共享库两种，静态库以 .a 作为后缀名，共享库以 .so 结尾。
静态库每次被调用都会生成一个副本，而共享库则只有一个副本。
如果可执行程序想调用库文件中的函数，它需要参考该库提供的头文件，以明白调用的格式。同时，要把可执行程序链接到库文件上。
只要拿到了头文件和库文件，就可以调用这个库了。
使用 IDE
IDE（Integrated Development Environment）为开发者提供了跳转、补全、断点调试等很多方便的功能。
在 Linux 中，默认的调试工具 gdb 只提供了文本界面，有些 IDE 提供了断点调试功能（底层仍旧是 gdb），Kdevelop 就是其中之一。
对于编译类型，通常有调试用的 Debug 模式与发布用的 Release 模式。启动器：既可以直接选择一个 cmake 的工程目标，也可以直接指向一个二进制文件（推荐）。
在断点处，可以用单步运行（F10）、单步跟进（F11）、单步跳出（F12）功能控制程序的运行。