书接上回: 第一讲
第二讲 初识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 t t = 1，···， K K K当中发生的事情。在这些时刻，用 x x x表示小萝卜自身的位置。于是各时刻的位置就记为 x 1 x_{1} x1，···， x K x_{K} xK，它们构成了小萝卜的轨迹。在地图方面，我们假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 N N N个，用 y 1 y_{1} y1，···， y N y_{N} yN表示它们。
那么，在上述设定中，“小萝卜的运动”可以由以下两件事情描述：
- 什么是运动？我们要考察从 k − 1 k-1 k−1时刻到 k k k时刻，小萝卜的位置 x x x是如何变化的。
- 什么是观测？假设小萝卜在 k k k时刻于 x k x_{k} xk处探测到了某一个路标 y j y_{j} yj，我们要考虑如何用数学语言来描述这件事情。
在运动的过程中，我们通常能使用一个通用的，抽象的数学模型来说明此事：
x k = f ( x k − 1 , u k , w k ) . x_{k} = f(x_{k-1},u_k,w_k). xk=f(xk−1,uk,wk).
这里， u k u_{k} uk是运动传感器的读数或者输入， w k w_{k} wk为该过程中加入的噪声。我们称该方程为运动方程。并且因为有了噪声的存在，该模型变成了随机模型。不然如果所有指令都是准确无误的，那就没有必要去估计了。
与运动方程相对应的，有一个观测方程。其描述是，当小萝卜在 x k x_{k} xk位置上看到某个路标点 y j y_{j} yj时，产生了一个观测数据 z k ， j z_{k，j} zk，j。同样，用一个抽象的函数h来描述这个关系：
z k , j = h ( y j , x k , v k , j ) . z_{k,j} = h(y_{j},x_k,v_{k,j}). zk,j=h(yj,xk,vk,j).
这里， v k , j v_{k,j} vk,j是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据 z z z及观测方程 h h h也会有许多不同的形式。
此外，根据小萝卜的真实运动和传感器的种类，存在着若干种参数化方式。例如，在平面下运动时，它的位姿（位置和姿态）由两个位置和一个转角来描述，即 x k = [ x 1 , x 2 , θ ] k T x_{k} = [x_1,x_2,θ]_k^T xk=[x1,x2,θ]kT,其中 x 1 x_{1} x1， x 2 x_{2} x2是两个轴上的位置，而 θ θ θ为转角。同时，输入的指令是两个时间间隔位置和转角的变化量 u k = [ Δ x 1 , Δ x 2 , Δ θ ] k T u_{k} = [Δx_1,Δx_2,Δθ]_k^T uk=[Δx1,Δx2,Δθ]kT，于是，此时运动方程就可以具体化为
[ x 1 x 2 θ ] k \begin{bmatrix}x_1\\x_2\\θ\end{bmatrix}_k ⎣⎡x1x2θ⎦⎤k = [ x 1 x 2 θ ] k − 1 \begin{bmatrix}x_1\\x_2\\θ\end{bmatrix}_{k - 1} ⎣⎡x1x2θ⎦⎤k−1 + [ Δ x 1 Δ x 2 Δ θ ] k \begin{bmatrix}Δx_1\\Δx_2\\Δθ\end{bmatrix}_k ⎣⎡Δx1Δx2Δθ⎦⎤k + w k w_k wk.
当然上述式子只是简单的线性关系。还存在其他形式更加复杂的运动方程，那时我们就可能需要进行动力学分析。
关于观测方程，以小萝卜携带着的一个二维激光床干起为例。激光传感器观测一个2D路标点时，能够测到两个量：路标点与小萝卜本体之间的距离 r r r和夹角 ø ø ø 。计路标点为 y j = [ y 1 , y 2 ] j T y_{j} = [y_1,y_2]_j^T yj=[y1,y2]jT，位姿为 x k = [ x 1 , x 2 ] k T x_{k} = [x_1,x_2]_k^T xk=[x1,x2]kT,观测数据为 z k , j = [ r k , j , ø k , j ] T z_{k,j} = [r_{k,j},ø_{k,j}]^T zk,j=[rk,j,øk,j]T,那么观测方程就写为
[ r k , j ø k , j ] \begin{bmatrix}r_{k,j}\\ø_{k,j}\end{bmatrix} [rk,jøk,j] = [ ( y 1 , j − x 1 , k ) 2 + ( y 2 , j − x 2 , k ) 2 a r c t a n ( y 2 , j − x 2 , k y 1 , j − x 1 , k ) ] \begin{bmatrix}\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{bmatrix} [(y1,j−x1,k)2+(y2,j−x2,k)2arctan(y1,j−x1,ky2,j−x2,k)]+ v v v.
如果我们保持通用性，把它们取成通用的抽象形式，那么SLAM过程可总结为两个基本方程
{ x k = f ( x k − 1 , u k , w k ) ， k = 1 , ⋅ ⋅ ⋅ , K . z k , j = h ( y j , x k , v k , j ) ， ( k , j ) ∈ O \begin{cases} x_{k} = f(x_{k-1},u_k,w_k)，k=1,···,K.\\ z_{k,j} = h(y_{j},x_k,v_{k,j})，\ (k,j)∈O \end{cases} { xk=f(xk−1,uk,wk)，k=1,⋅⋅⋅,K.zk,j=h(yj,xk,vk,j)， (k,j)∈O
其中 O O O是一个集合，记录着在哪个个时刻观察到了哪个路标。这就描述了最基本的SLAM问题：当知道运动测量的读数 u u u，以及传感器的读数 z z z时，如何求解定位问题（估计 x x x）和建图问题（估计 y y 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 <iostream>
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 <iostream>
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] 和 [14],你能看懂文献的内容吗?
文献[1]主要专注于对基于单目视觉的 SLAM 方法的分析和讨论，介绍了基于滤波、关键帧BA和直接跟踪这三类目前主流的单目 V-SLAM 方法的优缺点并对它们的代表性系统进行性能分析和比较，然后介绍和讨论了 V-SLAM 技术的最新研究热点和发展趋势, 并进行总结和展望。
文献[14]从帧间配准、环形闭合检测以及图优化技术3方面出发, 对基于图优化的SLAM技术进行综述 -
*阅读 SLAM 的综述文献,例如 [9, 15, 16, 17, 18] 等。这些文献关于 SLAM 的看法与本书有何异同?
文献[9]将SLAM发展分为三个年代，目前处于鲁棒性时代。
文献[15]大同小异。
文献[16]介绍了拓扑地图，以解决度量地图计算量大的问题。
文献[17]卡尔曼的介绍，
文献[18]分类和介绍视觉SLAM技术的四个主要框架：卡尔曼滤波器（KF)为基础，圆锥滤波器(PF)为基础，基于期望最大化(EM)和基于成员资格的方案. -
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 -
使用 build 文件夹来编译你的 cmake 工程,然后在 Kdevelop 中试试。
用的CLion -
刻意在代码中添加一些语法错误,看看编译会生成什么样的信息。你能看懂 g++ 的错误吗?
类似以前在VC++和CFree中写C语言的代码一样，只要是计算机科班应该都看得懂。 -
如果忘了把库链接到可执行程序上,编译会报错吗?什么样的错?
会。undefined reference to … -
*阅读《cmake 实践》，了解 cmake 的其他语法。
抽空看看。 -
*完善 hello SLAM 的小程序,把它做成一个小程序库,安装到本地硬盘中。然后，新建一个工程,使用 find_package 找这个库并调用它。
略。 -
*寻找其他 cmake 教学材料,深入了解 cmake,例如https://github.com/TheErk/CMake-tutorial。
收藏了，希望能用到。 -
寻找 Kdevelop 的官方网站,看看它还有哪些特性。你都用上了吗?
用的CLion -
如果你在上一讲学习了 vim,请试试 Kdevelop 的 vim 编辑功能。
不试