视觉SLAM第2讲：初识SLAM

目录

2.1 引子：小萝卜的例子

2.2 经典视觉SLAM框架

2.3 SLAM问题的数学表达

2.4 实践：编程基础

---

 

学习目标

1.理解一个视觉SLAM框架由哪几个模块组成，各模块的任务是什么。

2.搭建编程环境，为开发和实验做准备。

3.理解如何在Linux下编译并运行一个程序，如果程序出了问题，又该如何调试它。

4.掌握cmake的基本使用方法。

2.1 引子：小萝卜的例子

2.1.1 引言

假设我们组装了一台叫做“小萝卜”的机器人，如图 2-1

                               

                           图2-1 小萝卜

我们给“小萝卜”装上轮子使其能够移动，但不加规划、控制的话小萝卜就只能四处乱走 ，而要规划和控制首先要感知周边的环境，为此给其装上相机。但要是小萝卜能够探索房间 ，其至少需要知道两件事：

（1）我在什么地方？——定位

（2）周围环境是什么样？——建图

2.1.2 定位与建图

       “定位”和“建图”，可以看成感知的“内外之分”。作为一个“内外兼修”的小萝卜，一方面要明白自身的状态（即位置)，另一方面也要了解外在的环境（即地图)。当然，解决这两个问题的方法非常多。例如，我们可以在房间地板上铺设导引线,在墙壁上贴识别二维码,在桌子上放置无线电定位设备（这其实是现在很多仓储物流机器人的做法)。如果在室外，还可以在小萝卜脑袋上安装GPS信号接收器（像手机或汽车一样)。有了这些东西，定位问题是否就解决了呢?我们不妨把这些传感器分为两类。

       一类传感器是携带于机器人本体上的，例如机器人的轮式编码器、相机、激光传感器,等等。另一类是安装于环境中的，例如前面讲的导轨、二维码标志，等等。安装于环境中的传感设备，通常能够直接测量机器人的位置信息，简单有效地解决定位问题。然而，由于它们要求环境必须由人工布置，在一定程度上限制了机器人的使用范围。例如，室内环境往往没有GPS信号，绝大多数园区无法铺设导轨，这时该怎么定位呢? 

       这类传感器约束了外部环境。只有在这些约束满足时，基于它们的定位方案才能工作。反之，当约束无法满足时，我们就无法进行定位。所以，虽然这类传感器简单可靠，但它们无法提供一个普遍的、通用的解决方案。相对地，那些携带于机器人本体上的传感器，比如激光传感器、相机、轮式编码器、惯性测量单元（Inertial Measurement Unit，IMU)等，它们测量的通常都是一些间接的物理量而不是直接的位置数据。例如，轮式编码器会测量轮子转动的角度，IMU测量运动的角速度和加速度，相机和激光传感器则读取外部环境的某种观测数据。我们只能通过一些间接的手段，从这些数据推算自己的位置。虽然听上去这是一种迂回战术，但更明显的好处是，它们没有对环境提出任何要求，从而使得这种定位方案可适用于未知环境。

2.1.3 相机分类

       视觉SLAM侧重关注小萝卜的眼睛能做些什么事，SLAM中使用的相机不同于平时见到的单反摄像头，其通常不携带昂贵的镜头，而是以一定速率拍摄周围的环境，形成一个连续的视频流。普通摄像头 能以每秒拍摄30张图片的速度采集图像，高速相机则更快一些。

       按工作方式相机分为单目(Monocular)相机、双目(Stereo）相机和深度（RGB-D)相机三大类，如图2-2。直观看来，单目相机只有一个摄像头，双目有两个，而 RGB-D除了能够采集到彩色图片，还能读出每个像素与相机之间的距离。深度相机通常携带多个摄像头，工作原理和普通相机不尽相同。此外，SLAM中还有全景相机、Event相机等特殊或新兴的种类。

                           图2-2 相机分类

1.单目相机

（1）定义

       只使用一个摄像头进行SLAM的做法称为单目SLAM ( Monocular SLAM )。这种传感器结
构特别简单，成本特别低，所以单目SLAM非常受研究者关注。其数据是照片。
（2）数据特点

        照片本质上是拍摄某个场景在相机的成像平面上留下的一个投影，它以二维的形式记录了三维的世界。该过程丢掉了场景的一个维度（深度和距离），因此我们无法通过单张图片计算场景中物体与相机之间的距离(远近)。
       这个距离将是SLAM中非常关键的信息。我们见过大量图像，形成了一种天生的直觉，对大部分场景都有一个直观的距离感（空间感），它可以帮助我们判断图像中物体的远近关系。
（3）数据处理的方向和原理

       由于单目相机拍摄的图像只是三维空间的二维投影，所以，如果真想恢复三维结构，必须改变相机的视角。在单目SLAM中也是同样的原理。我们必须移动相机，才能估计它的运动，同时估计场景中物体的远近和大小，不妨称之为结构。那么，怎么估计这些运动和结构呢?想象你坐在一辆运动的列车中。一方面，如果列车往右边移动,那么我们看到的东西就会往左边移动——这就给我们推测运动带来了信息。另一方面，我们还知道：近处的物体移动快，远处的物体移动慢，极远处（无穷远处）的物体（如太阳、月亮）看上去是不动的。于是，当相机移动时，这些物体在图像上的运动就形成了视差（Disparity)。通过视差，我们就能定量地判断哪些物体离得远，哪些物体离得近。

（4）弊端

       我们知道了物体远近，它们仍然只是一个相对的值。比如我们在看电影时，虽然能够知道电影场景中哪些物体比另一些大，但无法确定电影里那些物体的“真实尺度”：那些大楼是真实的高楼大厦，还是放在桌上的模型？而摧毁大厦的是真实怪兽，还是穿着特摄服装的演员?如果把相机的运动和场景大小同时放大两倍，单目相机所看到的像是一样的。同样地，把这个大小乘以任意倍数,我们都将看到一样的景象。这说明，单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度(Scale )。由于单目SLAM无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性(Scale Ambiguity)。
        平移之后才能计算深度，以及无法确定真实尺度，这两件事情给单目SLAM的应用造成了很大的麻烦。其根本原因是通过单张图像无法确定深度。所以，为了得到这个深度人们开始使用双目相机和深度相机。

2.双目相机和深度相机

       使用双目相机和深度相机的目的是通过某种手段测量物体与相机之间的距离，克服单目相机无法知道距离的缺点。一旦知道了距离，场景的三维结构就可以通过单个图像恢复，同时消除尺度不确定性。尽管都是为了测量距离，但双目相机与深度相机测量深度的原理是不一样的。

双目相机

（1）原理

      由两个单目相机组成，但这两个相机之间的距离【基线（Baseline)】是已知的，如图2-3。我们通过这个基线来估计每个像素的空间位置——这和人眼非常相似。我们人类可以通过左右眼图像的差异判断物体的远近，在计算机上也是同样的道理。如果对双目相机进行拓展，则可以搭建多目相机，不过本质上并没有什么不同。

图2-3 双目相机数据：左右眼图像 ，通过左右眼的差异，能够判断场景中物体与相机之间的距离

（2）特点

       双目相机需要大量的计算才能（不太可靠地）估计每一个像素点的深度。双目相机测量到的深度范围与基线相关。基线距离越大，能够测量到的物体就越远，所以无人车上搭载的双目相机通常会是个很大的家伙。

       双目相机的距离估计是比较左右眼的图像获得的，并不依赖其他传感设备，所以它既可以应用在室内，又可应用于室外。

        双目或多目相机的缺点是配置与标定均较为复杂，其深度量程和精度受双目的基线与分辨率所限,而且视差的计算非常消耗计算资源，需要使用GPU和 FPGA设备加速，才能实时输出整张图像的距离信息。因此在现有的条件下，计算量是双目的主要问题之一。

深度相机

（1）原理

       可以通过红外结构光或Time-of-Flight (ToF)原理，像激光传感器那样，通过主动向物体发射光并接收返回的光，测出物体与相机之间的距离。它并不像双目相机那样通过软件计算来解决，而是通过物理的测量手段，所以相比于双目相机可节省大量的计算资源。如图2-4。

       

     图2-4 RGB-D数据：深度相机可以直接测量物体的图像和距离，从而恢复三维结构

（2）应用

       目前常用的RGB-D相机包括Kinect/Kinect V2、Xtion Pro Live 深度相机(又称RGB-D相机)。在一些手机上人们用它来识别人脸，在SLAM方面，主要用于室内，室外则较难应用。

（3）特点

       现在多数RGB-D相机还存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题。 

2.2 经典视觉SLAM框架

整个视觉SLAM流程包括以下步骤，如图2-5，其适用于静态、刚体、光照变化不明显、没有人为干扰的场景。

 

               图2-5 经典的视觉SLAM框架
1.传感器信息读取

在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
2.前端视觉里程计（Visual Odometry，VO)

视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端（Front End)。
3.后端（非线性）优化(Optimization)

后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化得到全局一致的轨迹和地图。由于接在VO之后，又称为后端(Back End)。
4.回环检测(Loop Closure Detection)

回环检测判断机器人是否到达过先前的位置。如果检测到回环,它会把信息提供给后端进行处理。
5.建图(Mapping)

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

2.2.1 视觉里程计

1.定义

视觉里程计能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。称它为“里程计”是因为它和实际的里程计一样，只计算相邻时刻的运动，而和过去的信息没有关联。

 2.引言

       如图2-6，人的感官上可以很自然的判断出右图是左图向左旋转一定角度的结果，但无法确定具体的运动参数（旋转角度、平移尺寸）；在计算机视觉领域，图像只是一个数值矩阵，其表达的含义计算机一无所知（机器学习所研究的问题）；在视觉SLAM中，我们只能看到一个个像素，知道它们是某些空间点在相机的成像平面上投影的结果，所以为了定量地估计相机运动，必须先了解相机与空间点的几何关系。

                         图2-6 图像示例

3.几何关系及视觉里程计的实现方法

       假定有一个视觉里程计，已经估计了两张图像间的相机运动。那么，一方面，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。

        但是，仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移(Accumulating Drift )。这是由于视觉里程计在最简单的情况下只估计两个图像间的运动造成的。我们知道，每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻，导致经过一段时间之后，估计的轨迹将不再准确，如图2-7。

         图2-7 累计误差与回环检测的矫正结果

       这也就是所谓的漂移(Drift )。它将导致我们无法建立一致的地图。你会发现原本直的走廊变成了斜的，而原本90°的直角不再是90°。为了解决漂移问题，我们还需要两种技术：后端优化和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。

2.2.2 后端优化

       处理SLAM过程中的噪声问题。便宜的传感器测量误差较大，昂贵的可能会小一些，有的传感器还会受磁场、温度的影响。所以，除了解决“如何从图像估计出相机运动”，我们还要关心这个估计带有多大的噪声，这些噪声是如何从上一时刻传递到下一时刻的，而我们又对当前的估计有多大的自信。后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计( Maximum-a-Posteriori,MAP)。这里的状态既包括机器人自身的轨迹，也包含地图。

       视觉里程计部分有时被称为“前端”。在SLAM框架中，前端给后端提供待优化的数据，以及这些数据的初始值。而后端负责整体的优化过程，它往往面对的只有数据，不必关心这些数据到底来自什么传感器。在视觉SLAM 中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。

2.2.3 回环检测

1.定义

       回环检测，又称闭环检测，主要解决位置估计随时间漂移的问题。假设实际情况下机器人经过一段时间的运动后回到了原点，但是由于漂移，它的位置估计值却没有回到原点。通过某种手段，让机器人知道“回到了原点”这件事，或者把“原点”识别出来，我们再把位置估计值“拉”过去，就可以消除漂移了。这就是所谓的回环检测。

2.回环检测与“定位”、“建图”的关系

       地图存在的主要意义是让机器人知晓自己到过的地方。为了实现回环检测，我们需要让机器人具有识别到过的场景的能力。它的实现手段有很多。例如像前面说的那样：

（1）在机器人下方设置一个标志物(如一张二维码图片)。它只要看到了这个标志，就知道自己回到了原点。该标志物实质上是一种环境中的传感器，对应用环境做了限制。

（2）我们更希望机器人能使用携带的传感器——也就是图像本身，来完成这一任务。例如，可以判断图像间的相似性来完成回环检测。这一点和人是相似的，当我们看到两张相似的图片时，容易辨认它们来自同一个地方如果回环检测成功，则可以显著地减小累积误差。所以，视觉回环检测实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富，使得正确检测回环的难度降低了不少。
       在检测到回环之后，我们会把“A与B是同一个点”这样的信息告诉后端优化算法。然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如果我们有充分而且正确的回环检测，则可以消除累积误差，得到全局一致的轨迹和地图。

 

2.2.4 建图

       建图是指构建地图的过程。地图（如图2-8）是对环境的描述，但这个描述并不是固定的，需要视SLAM的应用而定。 

                         图2-8 各类地图

       家用扫地机器人主要在低矮平面里面运动，只需一个二维地图标记哪里可以通过、哪里存在障碍物就可以在一定范围内导航了；对于相机而言，它有6个自由度的运动，至少需要一张三维地图……

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

1.度量地图（Metric Map）

       度量地图强调精确地表示地图中物体的位置关系，通常用稀疏(Sparse)与稠密(Dense)对
其分类。稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标，那么—张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略。相对地，稠密地图着重于建模所有看到的东西。定位时用稀疏路标地图就足够了。而用于导航时，则往往需要稠密地图（否则撞上两个路标之间的墙怎么办?)。

       稠密地图通常按照某种分辨率，由许多个小块组成，在二维度量地图中体现为许多个小格子(Grid )，而在三维度量地图中则体现为许多小方块( Voxel )。通常，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。当查询某个空间位置时，地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法，如A*、D*等，为机器人研究者所重视。但是我们也看到，一方面，这种地图需要存储每一个格点的状态，会耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。另一方面，大规模度量地图有时会出现一致性问题很小的一点转向误差，可能会导致两间屋子的墙出现重叠,使地图失效。

2.拓扑地图 （Topological Map）

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

2.3 SLAM问题的数学表达

       假设小萝卜正携带者某种传感器在未知环境里运动，由于相机通常是在某些时刻采集数据的，所以只关心这些时刻的位置和地图，即把一段连续时间的运动变成离散时刻t=1，……，K中发生的事。x表示小萝卜自身的位置， 表示各个时刻的位置。地图方面假设由许多路标组成，每个时刻传感器会测量到一部分标点，得到观测数据，路标点有N个、表示。

在这样的设定中，需要解决两个问题，用数学语言描述：

（1）运动：k-1时刻到k时刻小萝卜位置x的变化；

（2）观测：假设小萝卜在k时刻于处探测到路标。

       通常机器人会携带一个测量自身运动的传感器（码盘、惯性传感器），通过建立数学模型来表达其运动：，其中 f为通用运动方程、指运动传感器的读数或输入、指过程中加入的噪声。

       与运动方程相对应的是观测方程，其描述的是，当小萝卜在位置上看到路标点，此时产生一个观测数据，记为，其中指观测里的噪声。

2.4 实践：编程基础

2.4.1 安装Linux操作系统

本人在虚拟机VMare上使用Ubuntu18.04做为开发环境，虚拟机VMare、Ubuntu18.04的安装哔站上有很多教程，此处不再赘述，如有需要Ubunbu18.04镜像源的可私信我。

2.4.2 Hello SLAM

1.编写cpp文件

PS.

cpp、CMakeLists文件编写时牵涉到vim的指令使用，见快捷键中第9条：

Ubuntu18.04/Linux常用快捷键_ubuntu18.04切换中英文快捷键-CSDN博客

#include <iostream>
using namespace std;
​
int main(int argc, char const *argv[])
{
    cout << "hello Slam" << endl;
    return 0;
}

2.安装g++

sudo apt-get install g++

3.编译cpp文件为可执行文件

g++ helloSLAM.cpp

4.程序运行

./a.out

2.4.3 使用cmake

1.引言

       理论上，任意一个C++程序都可以用g++来编译。但当程序规模越来越大时，一个工程可能有许多个文件夹和源文件，这时输入的编译命令将越来越长。通常，一个小型C++项目可能含有十几个类，各类间还存在着复杂的依赖关系。其中一部分要编译成可执行文件，另一部分编译成库文件。如果仅靠g++命令，则需要输入大量的编译指令，整个编译过程会变得异常烦琐。因此，对于C++项目，使用一些工程管理工具会更加高效。在历史上，工程师们曾使用makefile进行自动编译，cmake比它更加方便。并且 cmake在工程上广泛使用。我们会看到后面提到的大多数库都使用cmake管理源代码。

2.cmake使用

（1）在一个cmake 工程中，我们会用cmake命令生成一个makefile文件，然后用make命令根据这个makefile文件的内容编译整个工程。以2.4.2 helloSLAM.cpp为例，这次我们不是直接使用g++，而是用cmake来制作一个工程，然后编译它。在目录下面新建CMakeLists.txt文件，内容如下。

#声明要求cmake的最低版本
cmake_minimum_required(VERSION 3.0.2)

#声明一个cmake工程
project(HelloSLAM)

#添加一个可执行程序，可执行文件名 cpp文件名
add_executable(helloSLAM helloSLAM.cpp)

（2）cmake编译

cmake .

       cmake会输出一些编译信息，然后在当前目录下生成一些中间文件，其中最重要的就是MakeFile。由于MakeFile是自动生成的，我们不必修改它。

（3）make编译

make

（4）程序运行

./helloSLAM

（5）编译优化

mkdir build
cd build
cmake ..
make

2.4.4 使用库

1.定义

       在一个C++工程中，并不是所有代码都会编译成可执行文件。只有带有main函数的文件才会生成可执行程序。而另一些代码，我们只想把它们打包成一个东西，供其他程序调用。这个东西叫作库（ Library )。

2.库使用

（1）编写库

//这是一个库文件
#include <iostream>
using namespace std;
 
void printHello() {
  cout << "Hello SLAM" << endl;
}

 （2）修改CMakeLists.txt

#声明要求cmake的最低版本
cmake_minimum_required(VERSION 3.0.2)

#声明一个cmake工程
project(HelloSLAM)

#添加一个可执行程序，可执行文件名 cpp文件名
add_executable(helloSLAM helloSLAM.cpp)

#添加hello库
add_library(hello libHelloSLAM.cpp)

（3）cmake编译

cd build
cmake ..
make

这时，在build文件夹中就会生成一个libhello.a文件，这就是我们得到的库。

3.共享库

     在Linux中，库文件分成静态库和共享库两种。静态库以.a作为后缀名，共享库以.so结尾。所有库都是一些函数打包后的集合，差别在于静态库每次被调用都会生成一个副本，而共享库则只有一个副本，更省空间。如果想生成共享库而不是静态库，只需使用以下语句即可。

add_library( hello_shared SHARED libHelloSLAM.cpp )

此时得到的文件就是libhello_shared.so。

 

4.头文件

       库文件是一个压缩包，里面有编译好的二进制函数。如果仅有.a或.so库文件，我们并不知道里面的函数到底是什么，调用的形式又是什么样的。为了让别人（或者自己）使用这个库，我们需要提供一个头文件，说明这些库里都有些什么。因此，对于库的使用者，只要拿到了头文件和库文件，就可以调用这个库。下面编写libhello的头文件。

#ifndef LIBHELLOSLAM_H_
#define LIBHELLOSLAM_H_
// 上面的宏定义是为了防止重复引用这个头文件而引起的重定义错误
 
// 打印一句hello的函数
void printHello();
 
#endif

 

5.应用：调用printHello函数

（1）编写useHello.cpp

#include "libHelloSLAM.h"
 
// 使用 libHelloSLAM.h 中的 printHello() 函数
int main(int argc, char **argv) {
  printHello();
  return 0;
}

 （2）在CMakeLists.txt中添加可执行程序的生成命令，链接到刚才使用的库上

#声明要求cmake的最低版本
cmake_minimum_required(VERSION 3.0.2)

#声明一个cmake工程
project(HelloSLAM)

#添加一个可执行程序，可执行文件名 cpp文件名
add_executable(helloSLAM helloSLAM.cpp)

#添加hello库
add_library(hello libHelloSLAM.cpp)
#共享库
add_library(hello_shared SHARED libHelloSLAM.cpp)

#添加可执行程序调用hello库中函数
add_executable(useHello useHello.cpp)
#将库文件链接到可执行程序上
target_link_libraries(useHello hello_shared)

（3）cmake编译

cd build
cmake ..
make