【视觉SLAM十四讲】初识 SLAM

本文为视觉 SLAM 学习总结。欢迎交流

本讲内容概要

• 视觉 SLAM 框架
• 搭建编程环境
• 在 Linux 下编译运行程序，如何调试
• 掌握 cmake 的基本使用方法

内外兼修

SLAM 中有两个相互耦合的问题，定位和建图需要内外兼修：

• 准确的定位需要精确的地图
• 精确的地图来自准确的定位

传感器

有两类传感器，一种是安装在环境中，如二维码 Marker、GPS、导轨等；另一类携带于机器人本体，如 IMU、激光、相机等。但环境中的传感器限制了应用环境，因此我们需要机器人在未知的环境下进行定位建图。

激光和相机

相机有便宜、轻便、信息量丰富的特点，但也存在耗费计算量并且对工作条件有一定要求，如光线要充足，不能被遮挡，图像具有一定的纹理。因此相机暂时还不能完全替代激光。

相机

相机的本质是以二维方式记录了三维世界的信息，在这个过程中丢了一个维度——距离。

单目相机

在下图中，我们不知道手上的人是真人还是模型。

近处的物体移动得快，远处的物体移动的慢。我们可以根据这个原理辨别图像中的距离。

双目相机

下图中，双目左右眼的微小差异判断远近。推算计算量很大。

深度相机

物理手段测量深度，有结构光和 ToF 的方法，主动测量，功耗大，深度测量较准确。但是量程较小，易受干扰。

视觉 SLAM 框架

视觉里程计类似于里程计，通过得到两两之间的运动，并对其进行估计，叠加后得到整个运动轨迹。里程计有漂移的现象，我们还需要用后端进行优化。回环检测通过检测相机又回到之前经过的点，从而对轨迹进行调整，最终得到全局一致的轨迹。有时我们会对地图有要求，特征点的地图不足以让机器人进行导航和避障，这时就需要建图。

视觉里程计：

• 相邻图像估计相机运动
• 通过两张图像计算运动和结构
• 不可避免地有漂移

后端优化：

• 从带有噪声的数据中优化轨迹和地图，是状态估计问题
• 最大候验概率估计 MAP
• 前期以 EKF 为代表，现在以图优化为代表

回环检测：

• 检测机器人是否回到早先位置
• 计算图像间的相似性

建图：

• 用于导航、规划等
• 度量地图 vs 拓扑地图
• 稀疏地图 vs 稠密地图

SLAM 问题的数学描述

SLAM 是离散时间 $t=1,2,\dots ,k$ 下测得的机器人位置 $x_1,x_2,\dots ,x_k$，将它们看成随机变量，服从概率分布。因为小萝卜是从上一个时刻运动到下一个时刻的，因此其位置可以表示为
$$x_k=f(x_{k-1},u_k,w_k)$$
其中 $x_{k-1}$ 为上一时刻，$u_k$ 为输入，$w_k$ 为噪声。称其为运动方程。

在相机中，我们会观察到很多路标（三维空间点）$y_1,y_2,\dots y_n$，传感器在位置 $x_k$ 探测到了路标 $y_j$，得到了一个探测数据：
$$z_{k,j}=h(x_k,y_j,v_{k,j})$$
其中 $v_{k,j}$ 为噪声。称其为观测方程。

• $x_k$ 的表示形式在 CH3、4 讨论
• 观测数据是相机中的像素点，如何表述？在 CH5 讨论
• 已知输入 $u,z$，如何推断 $x,y$ 在 CH6 讨论

实践部分

运行环境

• Windows 10
• VMware Workstation Pro
• Ubuntu 16.04 LTS

g++

首先使用 touch main.cpp 在新建的 ch2 目录下创建文件 main.cpp，然后输入 vim main.cpp 进行编辑：

#include<iostream>
using namespace std;
int main(int argc, char** argv) {
	cout << "Hello SLAM" << endl;
    return 0;
}

按下 ESC 后在底部输入 :wq 保存文件并退出。然后用 g++ 进行编译，产生可执行文件并运行，输出 “Hello SLAM”。

g++ main.cpp # g++编译
ls
./a.out # 运行可执行文件

以上使用了默认的选项。我们可以用 -o 选项来指定生成的文件名：

g++ main.cpp -o helloSLAM
ls
./helloSLAM

cmake

当工程很大时，我们有很多 cpp 文件，如果使用 g++，我们需要输入很多参数十分麻烦。可以用 cmake 来编译整个工程。关于 Ubuntu 中 cmake 的安装见 Ubuntu 中安装 cmake。

首先创建文件 CMakeLists.txt，并添加代码：

cmake_minimum_required(VERSION 2.8) # 声明最低版本
project(helloSLAM) # 声明一个cmake工程

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

在命令行中输入：

# cmake后需指定路径，.表示当前路径下，生成一个makefile文件
cmake .

# 根据makefile文件的内容编译整个工程
make

最终得到可执行文件 helloslam。我们的工作从写一串的 g++ 变为了维护一个 CMakeLists.txt，但实际调用的还是 g++，当源文件很多时，这种方法的优势就很明显了。这种优势在后续的软件安装时你会明显的感受到。

在刚才命令 cmake . 时生成了很多中间文件与源文件夹杂在一起，显得文件结构很混乱，我们可以在当前目录下创建 build 文件夹，然后对上层的文件进行编译，中间文件会生成在 build 文件夹中，上层的文件夹是干净的。并且在发布源代码时，我们只需要删除 build 文件夹。

mkdir build
cd build
cmake ..
make

在 C++ 工程中，只有含有 main 函数的文件才会生成可执行程序。如果程序需要调用一些其他文件中的类或函数，就需要用的库来打包。我们写一个例子进行说明，通过调用其他函数来打印 Hello SLAM：

// hello.cpp
#include <iostream>
using namespace std;
void printHello(){
	cout<<"Hello SLAM"<<endl;
}

CMakeLists.txt 最后一行后加入：

add_library(hello hello.cpp) # 将hellp.cpp编译为hello的库

查看 build 目录下，产生了文件 libhello.a，是对 hello 进行了打包。如果要调用这个库，我们还需要有一个头文件告诉程序要调用这个库：

#pragma once

void printHello();

然后写一个 main 函数来调用这个 printHello 函数：

#include "hello.h"
int main(){
	printHello();
	return 0;
}

然后在 CMakeLists.txt 中添加一个可执行程序的生成命令，链接到刚才的库上：

add_executable(useHello useHello.cpp)
# useHello程序可调用hello库中的代码
target_link_libraries(useHello hello)

用别人的库也可以这样整合到自己的代码中。

使用 IDE

在目录的上下级之间跳转很复杂，我们可以用一个简单的 IDE 来完成这些工作，这里推荐 Kdevelop，使用以下命令进行安装：

sudo apt-get install kdevelop
kdevelop # 直接命令行打开

打开 Kdevelop 后，我们打开工程，选择刚才创建的 CMakeLists.txt 就可以打开刚才的工程了。可以点击“ctrl + 函数名”来跳转到对应的函数实现处。

在 IDE 中直接编译就会自动生成一个 build 目录，因此我一般会在编译器中编译，然后在命令行中运行可执行文件，方便查看结果。

在 CMakeLists.txt 中加入下面命令来设置编译模式：

set(CMake_BUILD_TYPR "Debug")

这里因为有多个可执行程序，我们需要指定需要调试的可执行程序。添加断点后，点击 Debug 模式就可以进行调试了。