视觉SLAM十四讲从理论到实践第1讲到第2讲的重点探索

前言

兜兜转转，笔者终于来到了当初学习Linux的本初目的–攻克视觉SLAM，当然Linux的学习自然是还没有完结的，但SLAM这边也到了不得不开始熟悉的时候了，笔者想借此留下自己的学习记录，以供查阅，顺便能和其他一起学SLAM的朋友共同进步。

一、何为SLAM

1.SLAM初识

SLAM是Simultaneous Localization and Mapping的缩写，即“同时定位与地图构建”，是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动，本书中介绍的传感器主要为相机，所以称“视觉SLAM”，从定义中可以注意到的是它要解决的问题有两个，一为“定位”，二为“地图构建”。完整的SLAM系统分成几个模块：视觉里程计、后端优化、建图，以及回环检测。需要学习必要的数学理论和许多编程只是，会用到Eigen、OpenCV、PCL、g2o、Ceres等库，并掌握它们在Linux操作系统中的使用方法。
本书的源代码均放在Github上：link
一个机器人-小萝卜的例子：希望其具有自主运动的能力，这时许多高级功能的前提，为此它需要感知周边的环境，能解决刚才所说的两个问题，一方面要明白自身的状态（即位置），另一方面要了解外资的环境（即地图），解决这两个问题方法众多，通常是引入传感器。

2.传感器之与视觉SLAM紧密相关的相机

上述传感器可分为两类，一类是携带于机器人本体上的，例如机器人的轮式编码器、相机、激光传感器另一类是安装与环境中的，如导轨、二维码标志等等，鉴于安装于环境中的传感器有对环境的特殊限制，只有在约束满足时，才能发挥其精准的性能，小萝卜的定位还是采用第一类，这类传感器测量的通常都是一些简介的物理量而不是直接的位置数据，至于本书的主角相机再包括激光传感器则读取外部环境的某种观测数据，从而推算自己的位置，更像是一种迂回战术，但其没有对环境提出任何要求。
小萝卜最终使用携带式传感器–相机，来完成SLAM（当谈论视觉SLAM时，我们解决问题采用的都是相机，虽然激光SLAM相对成熟）。SLAM中使用的相机更加简单，通常不携带昂贵的镜头，而是以一定速率拍摄周围的环境，形成一个连续的视频流。相机可以分为单目（Monocular）相机、双目（Stereo）相机和深度（RGB-D）相机三大类，深度相机除了能够采集到彩色图片，还能读出每个像素与相机之间的距离。

（1）.单目相机

照片的本质是以二维的形式记录了三维的世界，显然这个投影的过程中丢掉了深度（或距离）这个维度，而在单目相机中，我们无法通过单张图片计算场景中物体与相机之间的距离（远近）。
我们人通常能通过单张的图片区分物体的远近，但这是建立在一定经验的基础上的，在单张图像里，事实上是无法确定一个物体的真实大小的。我们必须移动相机，才能估计它的运动，同时估计场景中物体的远近和大小（称之为结构）。别忘了我们知道：近处的物体移动快，远处的物体移动慢，极远处的物体看上去是不动的（日月星辰之类）。当相机移动时，这些物体在图像上的运动就形成了视差（Disparity），可以通过这个判断物体远近。
弹幕SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是所谓的尺度，即知道了相对的大小关系却不知道绝对的，由于单目SLAM无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性。
总结下来，单目相机有两个弊端，平移之后才能计算深度，以及无法确定真实尺度。

（2）.双目相机和深度相机

双目相机与深度相机测量深度的原理是不同的。双目相机由两个单目相机组成，但这两个相机之间的距离（称为基线（Baseline））是已知的。我们通过这个集线来估计每个像素的空间位置–这和人眼非常相似。计算机上的双目相机需要大量的计算才能估计每一个像素点的深度，一点也不聪明。并且基线距离越大，能够测量到的物体就越远，所以无人车上搭载的双目相机通常很大。
双目相机和多目相机的缺点是配置与标定均较为复杂，其深度量程和精度受双目的基线与分辨率所限，而且视差的计算非常消耗计算资源。（计算量是双目的主要问题之一）
双目相机是通过软件计算，深度相机则是通过物理的测量手段，现在多数RGB-D相机还存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等问题。因此它在SLAM中，主要用于室内。

3.经典视觉SLAM框架

视觉SLAM并不是某种算法，它需要一个完整的算法框架。

（1）.传感器信息读取：

在视觉SLAM中主要为相机图像信息的读取和预处理

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

又称前端，其任务是估算相邻图像间相机的运动，以及局部地图的样子。
在计算机中，必须精确地测量某段运动信息，计算机如何通过图像确定相机的运动，因为图像在计算机里只是一个数值矩阵，要弄清矩阵表达什么，必须先了解相机与空间点的集合关系。
视觉里程计能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构，它只计算相邻时刻的运动，而和过去的信息没有关联。
估计两张图像间的相机运动，一方面，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题；另一方面，我们根据每个时刻的相机位置，计算出每个像素对应的空间点的位置，就得到了地图。
仅通过视觉里程计来估计轨迹，将不可避免地出现累计漂移（Accumulating Drift），因为每次估计都带有一定的误差，而由于里程计的工作方式，误差不断向下一时刻传递，一段时间后，估计的轨迹将不再准确，也因此需要后端优化和回环检测，回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。
前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等。

（3）.后端（非线性）优化（Optimization）：

又称后端，后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。
除了解决“如何从图像估计出相机运动”，我们还要关心这个估计带有多大的噪声，这些噪声是如何从上一时刻传递到下一刻的，后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态（包括机器人自身的轨迹，也包含地图），以及这个状态估计的不确定性有多大–这称为最大后验概率估计（Maximum-a-Posteriori，MAP）。
后端往往面对的只有数据，不必关心这些数据到底来自什么传感器。
后端主要是滤波与非线性优化算法。通过状态估计理论把定位和建图的不确定性表达出来后，采用滤波器和非线性优化，估计状态的均值和不确定性（方差）。

（4）.回环检测（Loop Closure Detection）：

用来判断机器人是否到达过先前的位置，如果检测到回环，它会把信息提供给后端进行处理。
为了实现此项，我们希望机器人能使用携带的传感器，即图像本身来实现识别到过的场景的能力。
例：可以判断图像间的相似性来实现回环检测，看是不是在同一个地方–视觉回环检测实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富，使得正确检测回环的难度降低了不少

（5）.建图（Mapping）：

根据估计的轨迹，建立与任务要求对应的地图。
地图大致上可以分为度量地图和拓扑地图。
1.度量地图：其强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse）与稠密（Dense）分类。定位时用系数路标地图就足够了，而用于导航时，则往往需要稠密地图，地图通常按照某种分辨率，由许多个小块组成，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体，其弊端在于浪费了大量存储空间存储没用到的地图信息，且容易出现一致性问题等。
2.拓扑地图：拓扑地图更强调地图元素之间的关系，它由节点和边组成，因为不关心两点之间的到达路径，它丢掉了地图的细节，放松了地图对精确位置的需要，但它确为一种更为紧凑的表达方式，当然它不擅长表达具有复杂结构的地图，它有很多相关问题，亟待研究。

二、实践操作

1.数学基础

用一个通用的、抽象的数学模型来说明机器人运动时的情况 ：
$x_k=f(x_{k-1},u_k,w_k)$ （所谓运动方程）
$x_k$为某一时刻下机器人的位置，$x_{k-1}$为上一时刻的位置，$u_k$是运动传感器的读数或者输入，$w_k$为该过程中加入的噪声。
与运动方程相对应有观测方程：
$z_{k,j}=h(y_j,x_k,v_{k,j})$
$y_j$表示机器人在$x_k$位置上看到的某个路标点，$z_{k,j}$是这时产生的一个观测数据，$v_{k,j})$是这次观测里的噪声。观测所有传感器形式更多，对应的观测数据与方程本身也有多种形式。
假设机器人在平面上运动，它的位姿用两个位置和一个转角来描述$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$$但并不是所有的输入指令都是位移和角度的变化量，针对不同的传感器，运动方程和观测方程有不同的参数化形式。

SLAM面临的状态估计问题是，如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量。其求解可以按观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。主流视觉SLAM使用以图优化（Graph Optimization）为代表的优化技术进行状态估计。

2.实践操作

前言里说到，笔者之前学习Linux是为学习视觉SLAM打基础的，因为大部分程序库只对Linux提供了较好的支持，而在Windows上的配置相当麻烦，本书也是用大家相对熟悉的Ubuntu系统作为开发环境。笔者采用当时安装好的优麒麟（双系统）进行实验。除了ubuntu官网许多高校的镜像站都提供了ubuntu软件源。这里不再赘述安装方法。

（1）.Hello SLAM

先进入/home/slambook2/ch2目录（mkdir -p 创建一下），然后输入vim helloSLAM.cpp，进入vim文本编辑器（笔者到目前为止，还没有进入专门讲vim的章节，但发现掌握几个命令即可完成基本操作），先由普通模式进入插入模式，按键按下a即可（还有很多按键可以进入插入模式），只有在插入模式下才能输入我们想要的程序。

#include <iostream>
using namespace std;

int main(int argc,char **argv)//似乎还没有用上这两个参数
{
	cout<<"Hello SLAM!"<<endl;
	return 0;
}

按下esc返回普通模式，按下：，输入wq，保存并退出，又回到终端的命令行界面，直接输入g++ helloSLAM.cpp即可对该文件进行编译，没有安装g++编译器的，还需输入sudo apt-get install g++安装。编译后会生成一个可执行文件a.out，输入./a.out即可执行。g++会默认把源文件编译成a.out这个文件名的程序，当然也可以指定这个输出的文件名，输入g++ -o xxxx helloSLAM.cpp即可。

（2）.使用cmake

理论上，任何一个C++程序都可以用g++来编译，但我们也注意到如果仅靠g++命令，当工程中包含的文件夹和源文件变多后，需要输入大量的编译指令，整个编译会变得异常繁琐。这时用cmake来制作一个工程。输入vim CMakeLists.txt，这个文件有它自己的语法，要准确输入才行。

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

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

#添加一个可执行程序
#语法：add_executable(程序名 源代码文件)
add_executable(helloSLAM helloSLAM.cpp)

它相当于是告诉cmake要对这个目录下的文件做什么事情。之后可以调用cmake对该工程进行cmake编译（如果cmake没有found，可能要按照终端的报错提示进行安装），输入cmake .，之后会在当前目录下生成一些中间文件，其中最重要的就是MakeFile，可以理解成一系列自动生成的编译指令。然后输入make对工程进行编译。顺利通过后便可以执行./helloSLAM。
执行cmake是处理了工程文件之间的关系，而执行make实际又是调用了g++来编译程序，项新增一个可执行文件，在CmakeLists.txt中添加语句即可，有了cmake帮助解决代码的依赖关系，无须输入一大串g++命令。

常规操作：mkdir build，cd build，cmake …，make(把cmake产生的中间文件与源代码分开存放)

（3）.使用库

为使用像OpenCV（提供了许多计算机视觉相关的算法），或像Eigen库（提供了矩阵代数的计算）这样的库，必须要会利用cmake生成库。
还是输入vim libHelloSLAM.cpp，先自己编写一个库文件
，这个库只提供了一个printHello的函数。

#include <iostream>
using namespace std;

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

在CmakeLists.txt中添加这个库，起名叫“hello”的库

add_library(hello libHelloSLAM.cpp)

按（2）中的常规操作进行后会在build文件夹中生成一个libhello.a的文件，这个是我们得到的库，这是一个静态库（以.a结尾），还可以用共享库，因为静态库每次被调用都会生成一个副本，而共享库只有一个副本。

add_library(hello_shared SHARED libHelloSLAM.cpp)

此时得到的库文件是libhello_shared.so。
只有库文件还不够，还要有一个头文件，说明库里都有什么，对于库的使用者，拿到库文件和头文件才能调用这个库。

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

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

#endif

下面重新写一串代码，用刚才写好的库函数和头文件来输出Hello SLAM。

#include "libHelloSLAM.h"
//使用libHelloSLAM.h中的printHello()函数
int main(int argc,char **argv)
{
	printHello();//注意这里没有添加<iostream>,直接写成cout反而不能实现
	return 0;
}

别忘了修改CmakeLists.txt文件。

add_executable(useHello useHello.cpp)

当然还需要紧接着添加一条链接的命令，让这个程序可以使用刚才的库。

target_link_libraries(useHello hello_shared)

小结：
1.先编写程序，库文件和头文件，在程序文件中调用的是头文件，头文件是把库文件说明白的文件，库文件中才存放着具体的操作
2.编写CmakeLists.txt文件，让程序文件可执行，让库文件可链接到程序文件上

（4）.使用IDE完成上述操作

Linux上的IDE也很方便，相比单个文本编辑器配合终端的操作更为人性化。书上推荐了两种，KDevelop（免费）和Clion。
笔者只尝试了前者，按下F8，（2）中的常规操作它都默认帮助完成了。打开Window中的Show Left Dock更有种亲切感，在我们的projects中进入到CmakeLists.txt中设置编译模式可以拥有断点调试功能。

set（CMAKE_BUILD_TYPE “Debug”）

还要打开运行（Run）–配置启动器（Configure Launches）–选中ch2–Add–useHello–选中工程目标（Project Target），下方的选项先不填，有时程序运行需要参数时，它们会作为main函数的参数被传入。单击OK设置完成，便可以放置断点了。

总结

我这些挪列可能也有错误，一切还是以书本和高翔教授的视频为准（视频）。到这里其实也还连入门都算不上，后面需要更多的数学知识，配合实践操作，大家一起加油。