视觉SLAM十四讲学习记录 第二讲

书接上回: 第一讲

第二讲 初识SLAM

2.1 引子：小萝卜的例子

  首先作者借“小萝卜”这类机器人引出了几个概念：

• 自主运动能力是很多高级功能的前提，需要定位与感知（建图）来规划。
• 定位：我在哪？即自身的状态。
• 建图：我周围长啥样？即了解环境。
• 传感器分为两类：一类是携带于机器人本体上的，如轮式编码器、相机、激光传感器等。
  另一类则是安装于环境中的。例如导轨、二维码标志。显然外部传感器限制了特定环境，相较而言前者更适用于未知环境。而这种测算外部环境的传感器一般通过间接的手段，推算出自己的位置。
• 视觉SLAM一般是指用相机来解决定位和建图的问题。
• 以工作方式的不同，相机可以分为单目相机、双目相机和深度相机三大类。单目就只能拍一个面的照片，如果移动起来，可以估计场景中的物体大小，但是没有一种尺度感。双目类似人眼，根据基线（两个相机的距离）来计算距离。但是计算量巨大，一般借助GPU和FPGA设备来加速计算。而深度相机（又称RGB-D相机）通过主动向物体发射光并接收返回的光，测出物体与相机之间的距离。这是一种物理手段，所以可以节省大量的计算资源。

2.2 经典视觉SLAM框架

  分为以下几个部分：传感器信息读取、前端视觉里程计、后端（非线性）优化、回环检测、建图。

2.2.1 视觉里程计

  视觉里程计主要关心相邻图像之间的相机运动。相机与空间点的几何关系：一方面，将相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，便于定位。另一方面，根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。
  但是仅仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移。鉴于其里程计般的工作方式，每一时刻的误差将会累积，之后的结果都会带上这一误差。这也就是所谓的漂移。所以引出了之后的两种技术：后端优化和回环检测。

2.2.2 后端优化

  后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计。这里的状态既包括机器人自身的轨迹，也包含地图。在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
  早期的SLAM问题是一个状态估计问题——正是后端优化的工作。在较早的SLAM相关论文中，学者称之为“空间状态不确定性的估计”（Spatial Uncertainty），即对运动主体自身和周围环境空间不确定性的估计。

2.2.3 回环检测

  回环检测，又称闭环检测，主要解决位置估计随时间漂移的问题。即使机器人知道“回到了原点”这件事，或者把“原点”识别出来，我们在将位置估计值“拉”回去，便可以消除漂移。
  要实现回环检测，我们需要让机器人具有识别到过的场景的能力。例如二维码的方法（显然对环境有一定的要求，起码要能贴），或者通过判断图像间的相似性来完成回环检测。后者与人很相似。就比如你从一个地方走到另一个地方，再原路返回，就是通过之前的环境与回来后的环境来对比判断是否返回到了原地。

2.2.4 建图

  建图是指构建地图的过程。但地图不是固定的，视周围环境和具体的SLAM应用场景而定。例如家用扫地机器人工作场景主要为低矮平面，故只需要一个二维的地图，来告诉机器人哪里可通过即可。但对相机而言，他有6自由度的运动，我们至少需要3D的地图。有时我们需要一个漂亮的重建结果，不仅是一组空间点，还需要带纹理的三角面片。有时，我们只要知道“A点到B点可以通过，而B点到C点不行”这些信息，甚至都不用地图，或者由他人提供（导航时的地图）。
  根据地图侧重点的不同，大体上分为两类：度量地图与拓扑地图。
  度量地图（Metric Map）：强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse）与稠密（Dense）对其分类。稀疏地图通常只选择表示一部分具有代表意义的东西，称之为路标。相反，稠密地图则会建模所有看到的东西。定位时用稀疏路标地图就好，导航时则需要稠密地图来避开路标间的障碍物。
  拓扑地图：强调地图元素之间的关系。拓扑地图是一个图，由节点和边组成，只考虑节点间的连通性，例如只关注A、B点是否连通，而不考虑两者间的路径。

2.3 SLAM问题的数学表达

  首先，我们要把一段连续时间的运动变成离散时刻$t$ = 1，···，$K$当中发生的事情。在这些时刻，用$x$表示小萝卜自身的位置。于是各时刻的位置就记为$x_1$，···，$x_K$，它们构成了小萝卜的轨迹。在地图方面，我们假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有$N$个，用$y_1$，···，$y_N$表示它们。
  那么，在上述设定中，“小萝卜的运动”可以由以下两件事情描述：

1. 什么是运动？我们要考察从$k-1$时刻到$k$时刻，小萝卜的位置$x$是如何变化的。
2. 什么是观测？假设小萝卜在$k$时刻于$x_k$处探测到了某一个路标$y_j$，我们要考虑如何用数学语言来描述这件事情。

  在运动的过程中，我们通常能使用一个通用的，抽象的数学模型来说明此事：

              $x_k=f(x_{k-1},u_k,w_k).$

  这里，$u_k$是运动传感器的读数或者输入，$w_k$为该过程中加入的噪声。我们称该方程为运动方程。并且因为有了噪声的存在，该模型变成了随机模型。不然如果所有指令都是准确无误的，那就没有必要去估计了。
  与运动方程相对应的，有一个观测方程。其描述是，当小萝卜在$x_k$位置上看到某个路标点$y_j$时，产生了一个观测数据$z_{k，j}$。同样，用一个抽象的函数h来描述这个关系：

              $z_{k,j}=h(y_j,x_k,v_{k,j}).$

  这里，$v_{k,j}$是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据$z$及观测方程$h$也会有许多不同的形式。
  此外，根据小萝卜的真实运动和传感器的种类，存在着若干种参数化方式。例如，在平面下运动时，它的位姿（位置和姿态）由两个位置和一个转角来描述，即$x_k=[x_1,x_2,\theta {]}_k^T$,其中$x_1$，$x_2$是两个轴上的位置，而$\theta$为转角。同时，输入的指令是两个时间间隔位置和转角的变化量$u_k=[\Delta x_1,\Delta x_2,\Delta \theta {]}_k^T$，于是，此时运动方程就可以具体化为

              ${\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_k$ = ${\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_{k-1}$ +${\left[\begin{array}{c}\Delta x_1\\ \Delta x_2\\ \Delta \theta \end{array}\right]}_k$ + $w_k$.

  当然上述式子只是简单的线性关系。还存在其他形式更加复杂的运动方程，那时我们就可能需要进行动力学分析。
  关于观测方程，以小萝卜携带着的一个二维激光床干起为例。激光传感器观测一个2D路标点时，能够测到两个量：路标点与小萝卜本体之间的距离$r$和夹角$ø$ 。计路标点为$y_j=[y_1,y_2{]}_j^T$，位姿为$x_k=[x_1,x_2{]}_k^T$,观测数据为$z_{k,j}=[r_{k,j},{ø}_{k,j}{]}^T$,那么观测方程就写为

              $\left[\begin{array}{c}{r}_{k,j}\\ {ø}_{k,j}\end{array}\right]$ = $\left[\begin{array}{c}\sqrt{(y_1{,}_j-x_1{,}_k{)}^2+(y_2{,}_j-x_2{,}_k{)}^2}\\ arctan(\frac{y_2{,}_j-x_2{,}_k}{y_1{,}_j-x_1{,}_k})\end{array}\right]$+ $v$.

  如果我们保持通用性，把它们取成通用的抽象形式，那么SLAM过程可总结为两个基本方程

              $\{\begin{array}{l}{x}_k=f(x_{k-1},u_k,w_k)，k=1,\cdot \cdot \cdot ,K.\\ z_{k,j}=h(y_j,x_k,v_{k,j})，(k,j)\in O\end{array}$

  其中$O$是一个集合，记录着在哪个个时刻观察到了哪个路标。这就描述了最基本的SLAM问题：当知道运动测量的读数$u$，以及传感器的读数$z$时，如何求解定位问题（估计$x$）和建图问题（估计$y$）?这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带噪声的测量数据，估计内部的、隐藏着的状态变量？
  状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关。按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。其中线性高斯系统（Linear Gaussian，LG系统）是最简单的，它的无偏的最优估计可以由卡尔曼滤波器（Kalman Filter，KF）给出。而在复杂的非线性非高斯系统（Non-Linear Non-Gaussian，NLNG系统）中，我们会使用以扩展卡尔曼滤波器（Extended Kalman Filter，EKF）和非线性优化两大类方法去求解。

2.4 实践：编程基础

2.4.1 安装Linux操作系统

  首先，我们要有一个安装了Ubuntu 18.04系统的电脑，并且具有一定的Linux命令基础。关于安装Ubuntu系统，我参考了一个CSDN博主帅中的小灰灰的文章，非常感谢，其中详细的讲述了如何在VMware虚拟机上安装Ubuntu 18.04。想必VMware虚拟机大家在本科阶段应该都有安装的经验，在此不予以赘述。
  安装完毕后，书中建议我们更换下软件源，方便软件以及库的更新下载。那么首推的肯定是清华源。这个我参考的是CSDN博主moneymyone的文章。
  再接着将代码下载下来，我是放到了“文档”目录下。

2.4.2 Hello SLAM

  这一节教我们如何在Linux系统上运行一个helloSLAM.cpp程序，例子给的是

#include 
using namespace std;

int main(int argc, char **argv) {
  cout << "Hello SLAM!" << endl;
  return 0;
}

首先要用g++编译器来将它编译成一个可执行文件：g++ helloSLAM.cpp
成功后会得到一个a.out文件，它具有可执行权限
在之后输入:./a.out就可以得到输出

2.4.3 使用 cmake

  理论上，任何一个C++程序都可以按照上述方法来编译，但是显然这样效率很低。所以我们要使用cmake帮我们管理源代码。

# 样例中的CMakeLists.txt
# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 2.8)

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

# 设置编译模式
set(CMAKE_BUILD_TYPE "Debug")

# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
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)

  这样我们也能得到一个可执行程序helloSLAM。运行结果也是一样的。
  当然其中的区别肯定是有的。我们先执行make过程实际调用了g++来编译程序。虽然这个过程中多了调用cmake和make的步骤，但我们对项目的编译管理工作，从输入一串g++命令，变成可维护若干个比较直观的CMakeLists.txt文件，这将明显降低维护整个工程的难度。例如，如果想新增一个可执行文件，只需在CMakeLists.txt中添加一行"add_executable"命令即可，而后续的步骤是不变的。
  不过这样我们的代码中会多出来一堆中间文件，这样肯定不好。所以我们最好是在一个单独文件中来生成中间文件。

mkdir build
cd build
cmake ..
make

这样当发布源代码时就可以将整个build文件删掉，这样就省事多了。

2.4.4 使用库

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

//这是一个库文件
#include 
using namespace std;

void printHello() {
  cout << "Hello SLAM" << endl;
}

因为这里面没有main函数，这意味着这个库中没有可执行文件。我们在CMakelists.txt里加上如下内容：
add_library( hello libHelloSLAM.cpp )
  这条命令告诉cmake，我们想把这个文件编译成一个叫做“hello”的库。然后，和上面一样，使用cmake编译整个工程。如此便会得到一个libhello.a文件，这就是我们得到的库。
  在Linux中，库文件分成静态库和共享库两种。静态库以.a作为后缀名，共享库以.so结尾。所有库都是一些函数打包后的集合，差别在于静态库每次被调用都会生成一个副本，而共享库则只有一个副本，更省空间。而如果想生成共享库而不是静态库，只需使用以下语句即可。
add_library( hello_shared SHARED libHelloSLAM.cpp )
此时得到的文件就是libhello_shared.so。
  库文件是一个压缩包，里面有编译好的二进制函数。如果仅有.a或.so库文件，那么我们并不知道并不知道里面的函数到底是什么，调用的形式又是怎么样的。为了方便他人使用这个库，我们需要提供一个头文件，说明这些库里都有些什么。因此，对于库的使用者，只要拿到了头文件和库文件，就可以调用这个库，就如该例的头文件libHelloSLAM.h：

#ifndef LIBHELLOSLAM_H_
#define LIBHELLOSLAM_H_
// 上面的宏定义是为了防止重复引用这个头文件而引起的重定义错误

// 打印一句hello的函数
void printHello();

#endif

之后再在另一个可执行程序上来调用这个简单的函数即可。然后，在CMakeLists.txt中添加一个可执行程序的生成命令，链接到刚才使用的库上：
add_executable(useHello useHello.cpp)
target_link_libraries(useHello hello_shared)

2.4.5 使用IDE

  建议使用KDevelop和Clion，前者免费，后者收费，根据需要选择。

课后习题

本章的课后习题大多是实践类型的，建议读者还是要去试试为好。有一些题参考的CSDN博主nullwh的文章，确实看文献这类的东西实在太费时间了。

1. 阅读文献 [1] 和 [14],你能看懂文献的内容吗?
   文献[1]主要专注于对基于单目视觉的 SLAM 方法的分析和讨论，介绍了基于滤波、关键帧BA和直接跟踪这三类目前主流的单目 V-SLAM 方法的优缺点并对它们的代表性系统进行性能分析和比较，然后介绍和讨论了 V-SLAM 技术的最新研究热点和发展趋势, 并进行总结和展望。
   文献[14]从帧间配准、环形闭合检测以及图优化技术3方面出发, 对基于图优化的SLAM技术进行综述

2. *阅读 SLAM 的综述文献,例如 [9, 15, 16, 17, 18] 等。这些文献关于 SLAM 的看法与本书有何异同?
   文献[9]将SLAM发展分为三个年代，目前处于鲁棒性时代。
   文献[15]大同小异。
   文献[16]介绍了拓扑地图，以解决度量地图计算量大的问题。
   文献[17]卡尔曼的介绍，
   文献[18]分类和介绍视觉SLAM技术的四个主要框架：卡尔曼滤波器（KF)为基础，圆锥滤波器(PF)为基础，基于期望最大化(EM)和基于成员资格的方案.

3. g++ 命令有哪些参数?怎么填写参数可以更改生成的程序文件名?

   gcc -E source_file.c
   -E，只执行到预编译。直接输出预编译结果。
   
   gcc -S source_file.c
   -S，只执行到源代码到汇编代码的转换，输出汇编代码。
   
   gcc -c source_file.c
   -c，只执行到编译，输出目标文件。
   
   gcc (-E/S/c/) source_file.c -o output_filename
   -o, 指定输出文件名，可以配合以上三种标签使用。
   -o 参数可以被省略。这种情况下编译器将使用以下默认名称输出：
   -E：预编译结果将被输出到标准输出端口（通常是显示器）
   -S：生成名为source_file.s的汇编代码
   -c：生成名为source_file.o的目标文件。
   无标签情况：生成名为a.out的可执行文件。
   
   gcc -g source_file.c
   -g，生成供调试用的可执行文件，可以在gdb中运行。由于文件中包含了调试信息因此运行效率很低，且文件也大不少。
   这里可以用strip命令重新将文件中debug信息删除。这是会发现生成的文件甚至比正常编译的输出更小了，这是因为strip
   把原先正常编译中的一些额外信息（如函数名之类）也删除了。用法为 strip a.out
   
   gcc -s source_file.c
   -s, 直接生成与运用strip同样效果的可执行文件（删除了所有符号信息）。
   
   gcc -O source_file.c
   -O（大写的字母O），编译器对代码进行自动优化编译，输出效率更高的可执行文件。
   -O 后面还可以跟上数字指定优化级别，如：gcc -O2 source_file.c，数字越大，越加优化。但是通常情况下，
   自动的东西都不是太聪明，太大的优化级别可能会使生成的文件产生一系列的bug。一般可选择2；3会有一定风险。
   
   gcc -Wall source_file.c
   -W，在编译中开启一些额外的警告（warning）信息。-Wall，将所有的警告信息全开。
   
   gcc source_file.c -L/path/to/lib -lxxx -I/path/to/include
   -l, 指定所使用到的函数库，本例中链接器会尝试链接名为libxxx.a的函数库。
   -L，指定函数库所在的文件夹，本例中链接器会尝试搜索/path/to/lib文件夹。
   -I, 指定头文件所在的文件夹，本例中预编译器会尝试搜索/path/to/include文件夹。
   
   使用－o来更改生成的文件名，如g++ -o hello hello.cpp
   

4. 使用 build 文件夹来编译你的 cmake 工程,然后在 Kdevelop 中试试。
   用的CLion

5. 刻意在代码中添加一些语法错误,看看编译会生成什么样的信息。你能看懂 g++ 的错误吗?
   类似以前在VC++和CFree中写C语言的代码一样，只要是计算机科班应该都看得懂。

6. 如果忘了把库链接到可执行程序上,编译会报错吗?什么样的错?
   会。undefined reference to …

7. *阅读《cmake 实践》，了解 cmake 的其他语法。
   抽空看看。

8. *完善 hello SLAM 的小程序,把它做成一个小程序库,安装到本地硬盘中。然后，新建一个工程,使用 find_package 找这个库并调用它。
   略。

9. *寻找其他 cmake 教学材料,深入了解 cmake,例如https://github.com/TheErk/CMake-tutorial。
   收藏了，希望能用到。

10. 寻找 Kdevelop 的官方网站,看看它还有哪些特性。你都用上了吗?
    用的CLion

11. 如果你在上一讲学习了 vim,请试试 Kdevelop 的 vim 编辑功能。
    不试