视觉SLAM从理论到实践：设计前端（1）

作者：高翔，张涛，刘毅，颜沁睿。
本文节选自图书《视觉SLAM十四讲：从理论到实践》

本文完全由实践部分组成，实际书写一个视觉里程计程序。你会管理局部的机器人轨迹与路标点，并体验一下一个软件框架是如何组成的。在操作过程中，我们会遇到许多问题：相机运动过快、图像模糊、误匹配……都会使算法失效。要让程序稳定运行，我们需要处理以上的种种情况，这将带来许多工程实现方面的、有益的讨论。

主要目标：

1. 实际设计一个视觉里程计前端。
2. 理解SLAM软件框架是如何搭建的。
3. 理解在前端设计中容易出现的问题，以及修补的方式。

搭建VO框架

知晓砖头和水泥的原理，并不代表能够建造伟大的宫殿。
在笔者深爱的《我的世界》游戏中，玩家拥有的只是一些色彩、纹理不同的方块。其性质极其简单，而玩家所要做的只是把这些方块放在空地上而已。理解一个方块至为简单，但实际拿起它们时，初学者往往只能搭建简单的火柴盒房屋，而有经验、有创造力的玩家则可用这些简单的方块建造民居、园林、楼台亭榭，乃至城市，如图1 。

图1 左下是笔者的练习作品。右下来自Epicwork团队作品：《圆明园》

在SLAM中，我们认为工程实现和理解算法原理应该至少是同等重要的，甚至更应强调如何书写实际可用的程序。算法的原理，就像一个个方块一样，我们可以清楚明确地讨论它们的原理和性质，但仅仅理解了一个个方块并不能使你建造真正的建筑：它们需要大量的尝试、时间和经验，我们鼓励读者朝更为实际的方向努力——当然这往往是十分复杂的。就像在《我的世界》里那样，你需要掌握各种立柱、墙面、屋顶的结构，墙面的雕花，几何形体角度的计算，这些远远不像讨论每个方块的性质那样简单。

SLAM的具体实现亦是如此，一个实用的程序会有很多的工程设计和技巧（Trick），还需要讨论每一步出现问题之后该如何处理。原则上讲，每个人实现的SLAM都会有所不同，多数时候我们并不能说哪种实现方式就一定是最好的。但是，我们通常会遇到一些共同的问题：“怎么管理地图点”“如何处理误匹配”“如何选择关键帧”，等等。我们希望读者能对这些可能出现的问题产生一些直观的感觉——我们认为这种感觉是非常重要的。

所以，出于对实践的重视，本章我们将带领读者领略一下搭建SLAM框架的过程。就像建筑那样，我们要讨论柱间距、门面宽高比等琐碎但重要的问题。SLAM工程是复杂的。即使我们只保留核心的部分，也会占用大量的篇幅，使本书变得过于繁冗。不过，请注意，尽管完成之后的工程是复杂的，但是中间的“由简到繁”的过程，是值得详细讨论、有学习价值的。所以，我们要从简单的数据结构出发，先来做一个简单的视觉里程计，再慢慢地把一些额外的功能加进来。换言之，我们要把从简单到复杂的过程展现给读者看，这样你才会明白一个库是如何像雪人那样慢慢堆起来的。
本讲的代码放在slambook/project中。由于随着开发过程不断前进，我们会对工程做一些删改，因此它的内容也会发生变化。所以我们会把中间的代码也保留在目录中，以版本号命名，以便读者随时查看、模仿。

确定程序框架

根据前两讲的内容，我们知道视觉里程计分单目、双目、RGB-D三大类。单目视觉相对复杂，而RGB-D最为简单，没有初始化，也没有尺度问题。本着由简入繁的指导思想，我们先从RGB-D做起。为了方便读者做实验，我们将使用数据集而非实际的RGB-D相机（因为不能保证读者人手一台RGB-D相机）。

首先，我们来了解一下Linux程序的组织方式。在编写一个小规模的库时，我们通常会建立一些文件夹，把源代码、头文件、文档、测试数据、配置文件、日志等分类存放，这样会显得很有条理。如果一个库内容很多，我们还会把代码分解成各个独立的小模块，以便测试。读者可以参照OpenCV或g2o的组织方式，看看一个大中型库是如何组织的。例如，OpenCV有core、imgproc、features2d等模块，每个模块分别负责不同的任务。g2o则有core、solvers、types等若干模块。不过在小型程序里，我们也可以把所有的东西糅在一起，称为SLAM库。

现在我们要写的SLAM库是一个小型库，目标是帮读者将本书用到的各种算法融会贯通，书写自己的SLAM程序。挑选一个工程目录，在其下面建立如下文件夹来组织代码文件：

1. bin 用来存放可执行的二进制文件。
2. include/myslam 存放SLAM模块的头文件，主要是.h文件。这种做法的理由是，当把包含目录设到include，引用自己的头文件时，需要写include “myslam/xxx.h”，这样不容易和别的库混淆。
3. src 存放源代码文件，主要是.cpp文件。
4. test 存放测试用的文件，也是.cpp文件。
5. lib 存放编译好的库文件。
6. config 存放配置文件。
7. cmake_modules 第三方库的cmake文件，在使用g2o之类的库时会用到它。

以上就是我们的目录结构，如图2所示。相比于之前每一讲内零零散散地放着的main.cpp，这种做法显得更有条理。接下来，我们会在这些目录里不断地添加新文件，逐渐形成一个完整的程序。

图2 工程项目的目录

确定基本数据结构

为了让程序跑起来，我们要设计好数据单元，以及程序处理的流程。这好比构成房屋的一个个的柱子和砖块。那么，在一个SLAM程序中，有哪些结构是最基本的呢？我们抽象出以下基本概念：

1. 帧：帧是相机采集到的图像单位。它主要包含一个图像（RGB-D情形下是一对图像）。此外，还有特征点、位姿、内参等信息。
   在视觉SLAM中我们会谈论关键帧（Key-frame）。由于相机采集的数据很多，存储所有的数据显然是不现实的。否则，如果相机放在桌上不动，程序的内存占用也会越来越高直至无法接受。通常的做法是把某些我们认为更重要的帧保存起来，并认为相机轨迹可以用这些关键帧来描述。关键帧如何选择是一个很大的问题，而且基于工程经验，很少有理论上的指导。在本书中我们会使用一个关键帧选择方法，但读者亦可考虑自己提出新的方式。
2. 路标：路标点即图像中的特征点。在相机运动后，我们还能估计它们的3D位置。通常，会把路标点放在一个地图当中，并将新来的帧与地图中的路标点进行匹配，估计相机位姿。

帧的位姿与路标的位置估计相当于一个局部的SLAM问题。除此之外，我们还需要一些工具，让程序写起来更流畅。例如：

1. 配置文件：在写程序过程中你会经常遇到各种各样的参数，比如，相机的内参、特征点的数量、匹配时选择的比例，等等。你可以把这些