《视觉SLAM十四讲第2版》学习笔记第2讲

发表于 2021-04-03 更新于 2021-04-18 分类于学习，《视觉SLAM十四讲》阅读次数：

本文字数： 8.2k 阅读时长 ≈ 27 分钟

前言

从0到1，进步的阶梯。——Hoohoo

之前买书的时候还没有到，用的就是第一版的电子书，有些内容也有改变，比如第2讲中的实验部分第1版使用的是Ubuntu14.04，而在第二版中已经使用了18.04，正好和我实验室电脑中装的一样，有些小确幸，当时没有赶时髦装20.04。

本讲开启了第1部分——数学基础，也是初识SLAM，那就让我们一起迈过0，进入1的世界吧！

学习笔记

主要目标

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

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

理解如何在 Linux 下编译并运行一个程序。如果它出了问题，我们又如何对它进行调试。

掌握 cmake 的基本使用方法。

感知主要为“定位”和“建图”——可以看做感知的内外之分。

即：状态（位置）、环境（地图）。

传感器主要分为两类：

携带于机器人本体上
安装于环境中

视觉SLAM主要是指如何用相机解决定位和建图问题。

相机种类

按照工作方式的不同，我们可以把相机分为三大类：单目（Monocular）、双目（Stereo）、**深度（RGB-D）**相机三大类。

单目相机

只有一个摄像头进行SLAM的做法成为单目SLAM。

优点：传感器构成简单、成本特别低。

缺点：平移之后才能计算深度、无法确定真实尺度，

数据：照片。

照片本质上是拍摄某个场景（Scene）在相机的成像平面上留下的一个投影。它以二维的形式记录了三维的世界。所以没有**深度（距离）**信息。

距离在SLAM中非常重要，因为人类的眼睛可以大概分辨出，但是机器无法分辨和估计。

由于单目相机拍摄的图像只是三维空间的二维投影，所以必须改变相机视角才能恢复三维结构。

需要拍许多张不同视角的照片进行估计。

我们移动相机，才能估计它的运动（Motion），同时估计场景中物体的远近和大小，称之为结构。

其中，近处的物体移动快，远处的物体移动慢，极远处（无穷远处）的物体——比如太阳月亮之类的看上去是不动的。于是相机在移动时，这些物体在图像上的运动就形成了视差（Disparity）。

但是即使我们知道了物体远近，它们只是一个相对的值。

想象我们在看电影时候，虽然能够知道电影场景中哪些物体比另一些大，但我们无法确定电影里那些物体的“真实尺度”:那些大楼是真实的高楼大厦，还是放在桌上的模型？而摧毁大厦的是真实怪兽，还是穿着特摄服装的演员？直观地说，如果把相机的运动和场景大小同时放大两倍，单目所看到的像是一样的。同样的，把这个大小乘以任意倍数，我们都将看到一样的景象。

单目 SLAM 估计的轨迹和地图，将与真实的轨迹、地图，相差一个因子，也就是所谓的尺度(Scale)。由于单目 SLAM 无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性。

双目相机

双目相机由两个单目相机组成，但是这两个相机之间的距离【基线（Baseline）】是已知的。和人的双线比较相似，通过这个基线来估计每个像素的空间距离。

你可以将你的一根手指放在你的鼻子前，分别捂住左右眼，被遮住的背景是不同的。

优点：距离估计是比较左右眼图像获得的，不依赖其他传感设备，所以室内室外都可以应用。

缺点：需要大量计算才能（不太可靠地）估计每一个像素点的深度值、配置和标定均较为复杂、其深度量程和精度受双目的基线与分辨率所致、计算视差非常耗费计算资源，需要使用GPU和FPGA设备加速。

深度相机

2010年后兴起，最大特点就是可以通过红外结构光和Time-of- Flight（ToF）原理，像激光传感器那样通过主动向物体发射反射光并接收返回的光（有点类似于回声啊哈哈）测距。

优点：它不像双目相机通过软件来计算，而是通过物理测量手段，所以计算量小。

缺点：测量范围窄、噪声大、视野小、易受日光干扰、无法测量投射材质等。

目前常用RGB-D相机：Kinect/Kinect V2、Xtion Pro Live、RealSense等。

经典视觉SLAM框架

如下图：

依靠这些算法，我们可以构建一个视觉SLAM系统，使之在正常的工作环境中实时定位和建图。

传感器信息读取

主要是为相机图像信息的读取与预处理。

前端视觉里程计（Visual Odometry，VO）

任务：估算相邻图像间相机的运动，以及局部地图的样子。VO又称前端（Front End）。

在计算机视觉中，根据图像确定相机的运动十分困难。图像在计算机中只是一个数值矩阵，在视觉SLAM中，我们只能看到一个个像素，知道它们是某些空间点在相机的成像平面上投影的结果，所以，为了定量地估计相机运动，必须先了解相机与空间点的几何关系。

称其为“里程计”是因为它和实际的里程计一样，只计算相邻时刻的运动，而和过去的信息没有关联。

仅仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移（Accumulating Drift）。这是由于视觉里程计在最简单的情况下只估计两个图像间的运动而造成的，因为每次估计都有一定误差，而由于里程计的工作方式，会将前刻的误差传递到下一刻，经过一段时间的累积后，估计的轨迹将不再准确。

如下图所示。

比方说，机器人先向左转90度，再向右转了90度。由于误差，我们把第一个90度估计成了89度。那我们就会尴尬地发现，向右转之后机器人的估计位置并没有回到原点。更糟糕的是，即使之后的估计再准确，与真实值相比，都会带上这-1度的误差。

这也就是所谓的漂移（Drift）。将会导致我们无法建立一致的地图。

为了解决漂移问题，我们还需要两种技术：后端优化和回环检测。

后端优化更多时候称之为后端。由于主要是用的是优化方法，故称为后端优化。

后端（非线性）优化

任务：处理SLAM过程中的噪声问题。

再精确的传感器也会带有误差，除了需要解决前端问题，还要关心这个估计带有多大噪声，是如何传递的，而我们又对当前的估计有多大信心。

后端优化就是如何从这些带有噪声的数据中估计整个系统的状态（包括机器人自身的轨迹，也包括地图），以及这个状态估计的不确定性有多大——即为最大后验概率估计（Maximum-a-Posteriori）。

在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波和非线性优化算法。

回环检测

回环检测又称为闭环检测。

任务：解决位置估计随时间漂移的问题。

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

回环检测和“定位”以及“建图”都有联系。地图存在的主要意义是让机器人知晓自己到过的地方，所以为了实现回环检测，我们需要让机器人具有识别到过的场景的能力。

尽量采用机器人自身携带的传感器实现，例如，可以判断图像间的相似性来完成回环检测。

检测到回环之后，我们会吧“A和B是同一个点”之类的信息告诉后端优化算法，让后端优化算法根据新信息进行调整消除误差。

建图

任务：构建地图。

地图（见下图）是对环境的描述，但是这个描述并不是固定的，需要视SLAM的应用而定。

对于地图我们有太多的想法和需求，但是建图并没有一个固定的形式和算法。

大体上讲，可以分为度量地图和拓扑地图。

度量地图

度量地图强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse）和稠密（Dense）对其分类。

稀疏地图进行了一定程度的抽象,并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标(Landmark)，那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略掉。相对的，稠密地图着重于建模所有看到的东西。对于定位来说，稀疏路标地图就足够了。而用于导航时，我们往往需要稠密的地图(否则撞上两个路标之间的墙怎么办？)。

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

拓扑地图

相比于度量地图的精确性，拓扑地图更强调地图元素之间的关系。

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

SLAM中的数学表达

我们要用数学语言来描述SLAM的过程，要用到一些变量和公式。

$t$ : 表示一个离散时刻

$x$ : 表示小萝卜自身的位置

$y$ : 表示路标

小萝卜携带着传感器在环境中运动，可以由下列两件事情描述：

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

先来看运动。通常，机器人会携带一个测量自身运动的传感器，比如说码盘或惯性传感器。这个传感器可以测量有关运动的读数，但不一定直接是位置之差，还可能是加速度、角速度等信息。有时候我们也会给小萝卜发送指定，比如“前进1m”“旋转90度”，或者“油门踩到底”“刹车”等。然而，无论是什么传感器，我们都能使用一个通用的、抽象的数学模型:

x_k = f(x_{k-1},u_k,w_k)\tag {2.1}

其中 $u_k$ 是运动传感器的读数或者输入， $w_k$ 为过程中加入的噪声。

噪声的存在使得这个模型变成了随机模型。

换句话说，即使我们下达“前进1m”的指令后，也不代表小萝卜真的前进了1m，如果所有的指令都是准确的，就没必要估计了。事实上，小萝卜可能这次前进了0.9m，下一次前进了1.1m，再一次可能根据轮胎打滑，干脆没前进。

于是每次运动过程中的噪声是随机的，如果我们不理会这个噪声，可能与实际位置相差十万八千里。

再来看观测。观测方程描述的是，当小萝卜在 $x_k$ 位置上看到某个路标点 $y_j$ 时，产生了一个观测数据 $z_{k,j}$ ，同样用一个抽象的函数 $h$ 来描述这个关系：

z_{k,j}=h(y_j,x_k,v_{k,j}) \tag{2.2}

这里， $v_{k,j}$ 是这次观测里的噪声。

事实上，根据小萝卜的真实运动和传感器的种类，存在着若干种参数化(Parameterization)方式。

什么叫参数化呢？

举例来说，假设小萝卜在平面中运动，那么，它的位姿由两个位置和一个转角来描述，即 $x_k= {[x_1, x_2, \theta]}^T_k$ 。其中 $x_1,x_2$ 是两个轴上的位置而 $\theta$ 是转角。同时，运动传感器能够测量到小萝卜在每两个时间间隔位置和转角的变化量 $u_k={[\bigtriangleup x_1,\bigtriangleup x_2,\bigtriangleup \theta]^T_k}$ 。

那么，此时运动方程就可以具体化为:

\begin{gathered} \begin{bmatrix} x_1 \\ x_2 \\ \theta \end{bmatrix}_k= \begin{bmatrix} x_1 \\ x_2 \\ \theta \end{bmatrix}_{k-1}= \begin{bmatrix} \bigtriangleup x_1 \\ \bigtriangleup x_2 \\ \bigtriangleup \theta \end{bmatrix}_k+w_k \end{gathered}\tag{2.3}

这是简单的线性关系。不过，并不是所有的输入指令都是位移和角度变化，例如“油门”或者“控制杆”的输入就是速度或者加速度量，所以也存在着其他形式更加复杂的运动方程，那时我们可能需要进行动力学分析。

关于观测方程，比方说小萝卜携带着一个二维激光传感器。我们知道激光传感器观测一个2D路标点时，能够测到两个量：路标点与小萝卜本体之间的距离 $r$ 和夹角 $\phi$ 。记路标点为 $y_j=[y_1,y_2]^T_j$ ，位姿为 $x_k=[x_1,x_2]^T_k$ 观测数据为 $z_{k,j}=[r_{k,j},\phi_{k,j}]^T$ ，那么观测方程就具体化为:

\begin{gathered} \begin{bmatrix} r_{k,j} \\ \phi_{k,j} \end{bmatrix}_k= \begin{bmatrix} \sqrt{(y_{1,j}-x_{1,k})^2-(y_{2,j}-x_{2,k})^2} \\ arctan(\dfrac{ {y_{2,j}-x_{2,k} } } { {y_{1,j}-x_{1,k} } }) \end{bmatrix}_{k-1}+v \end{gathered} \tag{2.4}

可见，针对不同的传感器，这两个方程有不同的参数化形式。如果我们保持通用性，把它们取成通用的抽象形式，那么SLAM过程可总结为两个基本方程:

\begin{gathered} \begin{matrix} \begin{cases} x_k=f(x_{k-1},u_k,w_k) ,\quad k = 1,\cdots,k \\ z_{k,j}=h(y_j,x_k,v_{k,j}) ,\quad (k,j) \in \mathcal{O} \end{cases} \end{matrix} \end{gathered} \tag{2.5}

其中 $\mathcal{O}$ 是一个集合，记录着在哪个时刻观察到了哪个路标。

这两个方程描述了最基本的SLAM问题：当我们知道运动测量的读数 $u$ ,以及传感器的读数 $z$ 时，如何求解定位问题(估计 $x$ )和建图问题(估计 $y$ )？

这时，我们把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量?

状态估计问题的求解于两个方程的具体形式，以及噪声服从哪种分部有关。

按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

其中线性高斯系统(Linear Gaussian, LG系统)是最简单的，它的无偏的最优估计可以由卡尔曼滤波器(Kalman Filter, KF)给出。而在复杂的非线性非高斯系统(Non-Linear Non-Gaussian, NLNG系统)中，我们会使用以扩展卡尔曼滤波器( Extended Kalman Filter, EKF)和非线性优化两大类方法去求解它。直至21世纪早期，以EKF为主的滤波器方法占据了SLAM中的主导地位。我们会在工作点处把系统线性化，并以预测更新两大步骤进行求解(见第九讲)。

最早的实时视觉SLAM系统即是基于EKF[2]开发的。随后，为了克服EKF的缺点(例如线性化误差和噪声高斯分布假设)，人们开始使用粒子滤波器( Particle Filter)等其他滤波器，乃至使用非线性优化的方法。时至今日，主流视觉SLAM使用以图优化( Graph Optimization)为代表的优化技术进行状态估计[13]。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，我们通常都偏向于使用优化方法。

实践：编程基础

cmake很重要，最近在做一些其他事情的时候，学到了一些皮毛，虽然不大懂，后期会去看《cmake实践》，详见前几篇blog可以看到cmake的身影。

2.4.1 - 2.4.3

使用库

以下图片为mac远程连接Ubuntu截图。

学习打包一个库文件。

库文件分为静态库和共享库。

静态库：以.a结尾

共享库：以.so结尾

区别：静态库每次被调用都会生成一个副本，而共享库只有一个副本。

编写libHelloSLAM.cpp文件。

在CMakeLists中添加一行add_library()
如果想要生成共享库，则输入
1
add_library(hello_shared SHARED libHelloSLAM.cpp)

cmake生成libhello.a

(其中ERROR部分为错误输入，不必看)

编写头文件，让人调用。

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

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

#endif

写一个可执行程序调用这个简单的函数

#include "libHelloSLAM.h"

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

在CMakeLists中添加一个可执行程序的生成命令，链接到刚才使用的库上。

执行：

IDE

个人使用的是CLion。

习题

阅读文献 [1] 和 [14]，你能看懂文献的内容吗？
*阅读 SLAM 的综述文献，例如 [9, 15, 16, 17, 18] 等。这些文献关于 SLAM 的看法与本书有何异同？
g++命令有哪些参数？怎么填写参数可以更改生成的程序文件名？
使用 build 文件夹来编译你的 cmake 工程，然后在 Kdevelop 中试试。
刻意在代码中添加一些语法错误，看看编译会生成什么样的信息。你能看懂 g++ 的错误吗？
如果忘了把库链接到可执行程序上，编译会报错吗？什么样的错？
*阅读《cmake实践》，了解cmake的其他语法。
* 完善 hello SLAM 的小程序，把它做成一个小程序库，安装到本地硬盘中。然后，新建一个工程，使用 find_package 找这个库并调用它。
*寻找其他cmake教学材料，深入了解cmake，例如https://github.com/TheErk/CMake-tutorial。
寻找 Kdevelop 的官方网站，看看它还有哪些特性。你都用上了吗？
如果你在上一讲学习了 vim，请试试 Kdevelop 的 vim 编辑功能。

----------到结尾啦！！ Hoohoo----------