目录
前言:
今天开始slam十四讲的学习,我将尽可能简化理解并分享给大家
视觉slam是什么:
SLAM是 Simultaneous Localization and Mapping 的缩写,中文译作“同时定位与地图构建”。它是指搭载特定传感器的主体,在没有环境先验信息的情况下,于运动过程中建立环境的模型,同时估计自己的运动。如果这里的传感器主要为相机,那就称为"视觉SLAM"。
引子:
首先引入经典的小萝卜机器人:
小萝卜具有自主运动能力并带有相机,那么使小萝卜能够探索一个房间,它至少需要知道两件事:
1.我在什么地方?——定位 定位侧重对自身的了解
2.周围环境是什么样?——建图 建图侧重对外在的了解
相互耦合的两个问题:1.准确的定位需要精确的地图
2.精确的地图来自准确的定位
传感器的分类
应用不同的传感器可以解决这两个问题,例如,我们可以在房间地板上铺设导引线,在墙壁上贴识别二维码,在桌子上放置无线电定位设备(这其实是现在很多仓储物流机器人的做法)。如果在室外,还可以在小萝卜脑袋上安装GPS信号接收器(像手机或汽车一样)。
我们把这些传感器分为两类,一类是安装于环境中的,例如前面讲的导轨、二维码标志,等等。安装于环境中的传感设备,通常能够直接测量机器人的位置信息,简单有效地解决定位问题。然而,由于它们要求环境必须由人工布置,在一定程度上限制了机器人的使用范围。例如,室内环境往往没有GPS ,绝大多数园区无法铺设导轨,这时该怎么定位呢?
这就要采用另一类携带于机器人本体上的传感器,比如激光传感器、相机、轮式编码器、惯性测量单元(Inertial Measurement Unit,IMU)等,它们测量的通常都是一些间接的物理量而不是直接的位置数据。例如,轮式编码器会测量轮子转动的角度,IMU测量运动的角速度和加速度,相机和激光传感器则读取外部环境的某种观测数据。我们只能通过一些间接的手段,从这些数据推算自己的位置。它们没有对环境提出任何要求,从而使得这种定位方案可适用于未知环境。
相机的分类
而回顾slam定义,我们在slam中非常强调未知环境。视觉slam是指如何用相机解决定位和建图问题,常用的相机可以分为单目(Monocular)相机、双目(Stereo)相机和深度(RGB-D)相机三大类。直观看来,单目相机只有一个摄像头,双目有两个,而 RGB-D的原理较复杂,除了能够采集到彩色图片,还能读出每个像素与相机之间的距离。深度相机通常携带多个摄像头,工作原理和普通相机不尽相同(第五讲)
单目相机
只使用一个摄像头进行SLAM的做法称为单目SLAM ( Monocular SLAM )。这种传感器结构特别简单,成本特别低,所以单目SLAM非常受研究者关注。
照片本质上是拍摄某个场景在相机的成像平面上留下的一个投影,它以二维的形式记录了三维的世界。显然,这个过程丢掉了场景的一个维度,也就是所谓的深度和距离,之后我们会看单目相机中,我们无法通过单张图片计算场景中物体与相机之间的距离(远近)。
如果真想恢复三维结构,我们必须移动相机,当相机移动时,这些物体在图像上的运动就形成了视差(Disparity)。通过视差,我们就能定量地判断哪些物体离得远,哪些物体离得近。
然而,即使我们知道了物体远近,但无法确定电影里那些物体的“真实尺度”:这说明,单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子,也就是所谓的尺度(Scale )。由于单目SLAM无法仅凭图像确定这个真实尺度,所以又称为尺度不确定性(Scale Ambiguity)。
总结:
平移之后才能计算深度,以及无法确定真实尺度,这两件事情给单目SLAM的应用造成了很大的麻烦。其根本原因是通过单张图像无法确定深度。所以,为了得到这个深度人们开始使用双目相机和深度相机。
双目相机和深度相机:
使用双目相机和深度相机的目的是通过某种手段测量物体与相机之间的距离,克服单目相机无法知道距离的缺点。一旦知道了距离,场景的三维结构就可以通过单个图像恢复,同时消除尺度不确定性。
尽管都是为了测量距离,但双目相机与深度相机测量深度的原理是不一样的。双目相机由两个单目相机组成,但这两个相机之间的距离[称为基线(Baseline)〕是已知的。我们通过这个基线来估计每个像素的空间位置——这和人眼非常相似。
计算机上的双目相机需要大量的计算才能(不太可靠地)估计每一个像素点的深度。双目相机测量到的深度范围与基线相关。基线距离越大,能够测量到的物体就越远,双目或多目相机的缺点是配置与标定均较为复杂,其深度量程和精度受双目的基线与分辨率所限,而且视差的计算非常消耗计算资源,需要使用GPU和 FPGA设备加速,才能实时输出整张图像的距离信息。因此在现有的条件下,计算量是双目的主要问题之一。
深度相机(又称RGB-D相机),它最大的特点是可以通过红外结构光或Time-of-Flight (ToF)原理,像激光传感器那样,通过主动向物体发射光并接收返回的光,测出物体与相机之间的距离。它并不像双目相机那样通过软件计算来解决,而是通过物理的测量手段,所以相比于双目相机可节省大量的计算资源。目前常用的RGB-D相机包括Kinect/Kinect V2、Xtion Pro Live 、RealSense等。不过,现在多数RGB-D相机还存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题,在SLAM方面,主要用于室内,室外则较难应用。
slam经典框架
通过在场景中运动的相机,得到一系列连续变化的图像。视觉slam通过这些图像,进行定位和地图构建,这很难,依靠某种算法不能实现,slam需要一个完善的算法框架。
整个视觉SLAM流程包括以下步骤。
1.传感器信息读取。在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中,还可能有码盘、惯性传感器等信息的读取和同步。
2.前端视觉里程计(Visual Odometry,VO)。视觉里程计的任务是估算相邻图像间相机的运动,以及局部地图的样子。VO又称为前端(Front End)。
3.后端(非线性)优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化得到全局一致的轨迹和地图。由于接在VO之后,又称为后端(Back End)。
4.回环检测(Loop Closure Detection)。回环检测判断机器人是否到达过先前的位置。如果检测到回环,它会把信息提供给后端进行处理。
5.建图(Mapping)。它根据估计的轨迹,建立与任务要求对应的地图。
此框架是过去十几年的研究成果,已经基本定型,如果把工作环境限定在静态、刚体、光照变化不明显、没有人为干扰的场景,slam的技术已经相当成熟。
视觉里程计
视觉里程计关心相邻图像之间的相机运动,最简单的情况当然是两张图像之间的运动关系。
直观来看,右图应该是左图向左旋转一定角度的结果,但如果问能否确定旋转了多少度,平移了多少厘米?我们就很难给出一个确切的答案...
为了定量地估计相机运动,必须先了解相机与空间点的几何关系,要讲清这个几何关系及视觉里程计的实现方法,需要铺垫一些背景知识。读者现在只需知道,视觉里程计能够通过相邻帧间的图像估计相机运动,并恢复场景的空间结构。称它为“里程计”是因为它和实际的里程计一样,只计算相邻时刻的运动,而和过去的信息没有关联。
现在,假定我们已有了一个视觉里程计,估计了两张图像间的相机运动。那么,一方面,只要把相邻时刻的运动“串”起来,就构成了机器人的运动轨迹,从而解决了定位问题。另一方面,我们根据每个时刻的相机位置,计算出各像素对应的空间点的位置,就得到了地图。这么说来,有了视觉里程计,是不是就解决了SLAM问题呢?
然而,仅通过视觉里程计来估计轨迹,将不可避免地出现累积漂移(Accumulating Drift )。这是由于视觉里程计在最简单的情况下只估计两个图像间的运动造成的。我们知道,每次估计都带有一定的误差,而由于里程计的工作方式,先前时刻的误差将会传递到下一时刻,导致经过一段时间之后,估计的轨迹将不再准确。
这也就是所谓的漂移(Drift )。它将导致我们无法建立一致的地图。你会发现原本直的走廊变成了斜的,而原本90°的直角不再是90°——这实在是一件令人难以忍受的事情!为了解决漂移问题,我们还需要两种技术:后端优化和回环检测。回环检测负责把“机器人回到原始位置”的事情检测出来,而后端优化则根据该信息,校正整个轨迹的形状。
后端优化
后端优化主要指处理SLAM过程中的噪声问题。虽然我们很希望所有的数据都是准确的,但是在现实中,再精确的传感器也带有一定的噪声。所以,除了解决“如何从图像估计出相机运动”,我们还要关心这个估计带有多大的噪声,这些噪声是如何从上一时刻传递到下一时刻的,而我们又对当前的估计有多大的自信。后端优化要考虑的问题,就是如何从这些带有噪声的数据中估计整个系统的状态,以及这个状态估计的不确定性有多大——这称为最大后验概率估计( Maximum-a-Posteriori,MAP)。这里的状态既包括机器人自身的轨迹,也包含地图。
视觉里程计部分有时被称为“前端”。在SLAM框架中,前端给后端提供待优化的数据,以及这些数据的初始值。而后端负责整体的优化过程,它往往面对的只有数据,不必关心这些数据到底来自什么传感器。在视觉SLAM 中,前端和计算机视觉研究领域更为相关,比如图像的特征提取与匹配等,后端则主要是滤波与非线性优化算法。
回环检测
回环检测,又称闭环检测,主要解决位置估计随时间漂移的问题。假设实际情况下机器人经过一段时间的运动后回到了原点,但是由于漂移,它的位置估计值却没有回到原点。如果有某种手段,让机器人知道“回到了原点”这件事,或者把“原点”识别出来,我们再把位置估计值“拉”过去,就可以消除漂移了。这就是所谓的回环检测。
我们认为,地图存在的主要意义是让机器人知晓自己到过的地方。为了实现回环检测,我们需要让机器人具有识别到过的场景的能力。
Ⅱ.我们更希望机器人能使用携带的传感器——也就是图像本身,来完成这一任务。例如,可以判断图像间的相似性来完成回环检测。这一点和人是相似的。当我们看到两张相似的图片时,容易辨认它们来自同一个地方。所以,视觉回环检测实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富,使得正确检测回环的难度降低了不少。
在检测到回环之后,我们会把“A与B是同一个点”这样的信息告诉后端优化算法。然后,后端根据这些新的信息,把轨迹和地图调整到符合回环检测结果的样子。这样,如果我们有充分而且正确的回环检测,则可以消除累积误差,得到全局一致的轨迹和地图。
建图
建图是指构建地图的过程。地图是对环境的描述,建图并没有一个固定的形式和算法。一组空间点的集合可以称为地图,一个漂亮的3D模型也是地图,一个标记着城市、村庄、铁路、河道的图片还是地图。地图的形式随SLAM的应用场合而定。大体上讲,可以分为度量地图与拓扑地图两种。
度量地图
度量地图强调精确地表示地图中物体的位置关系,通常用稀疏(Sparse)与稠密(Dense)对其分类。稀疏地图进行了一定程度的抽象,并不需要表达所有的物体。相对地,稠密地图着重于建模所有看到的东西。而用于导航时,则往往需要稠密地图
稠密地图通常按照某种分辨率,由许多个小块组成,在二维度量地图中体现为许多个小格子(Grid ),而在三维度量地图中则体现为许多小方块( Voxel )。通常,一个小块含有占据、空闲、未知三种状态,以表达该格内是否有物体。当查询某个空间位置时,地图能够给出该位置是否可以通过的信息。这样的地图可以用于各种导航算法,如A*、D*等,为机器人研究者所重视。
但是我们也看到,一方面,这种地图需要存储每一个格点的状态,会耗费大量的存储空间,而且多数情况下地图的许多细节部分是无用的。另一方面,大规模度量地图有时会出现一致性问题很小的一点转向误差,可能会导致两间屋子的墙出现重叠,使地图失效。
拓扑地图
相比于度量地图的精确性,拓扑地图更强调地图元素之间的关系。拓扑地图是一个图(Graph ),由节点和边组成,只考虑节点间的连通性,例如只关注A、B点是连通的,而不考虑如何从A点到达B点。它放松了地图对精确位置的需要,去掉了地图的细节,是一种更为紧凑的表达方式。然而,拓扑地图不擅长表达具有复杂结构的地图。如何对地图进行分割,形成节点与边,又如何使用拓扑地图进行导航与路径规划,仍是有待研究的问题。
slam问题的数学表述
假设小萝卜正携带某种传感器在未知环境里运动,如何用数学语言来描述这件事?
一段连续时间的运动变成离散时刻t=1,....,k中发生的事情,x表示小萝卜自身的位置,各时刻的位置记为x1,x2,....,xk。它们构成小萝卜的轨迹。观测数据由路标点y1,....,yn来表示。
运动方程和观测方程
在这样的设定中,运动和观测分别怎样来描述?
先看运动:机器人会携带一个传感器,传感器测量有关运动的读数。无论是什么传感器,我们都能使用一个通用的、抽象的数学模型:
uk为传感器的读数(输入),wk为噪声,此方程描述了从k-1时刻到k时刻,小萝卜位置x的变化,我们称它为运动方程。
再来看观测:
vk,j是这里的噪声,此方程描述了小萝卜在xk位置看到某个路标点yj,产生了一个观测数据zk,j。我们把它称为观测方程。
针对不同的传感器,有若干种参数化方式,但保持通用性,slam过程总结为两个基本方程:
这两个方程描述了最基本的slam问题:当知道运动测量的读数u,以及传感器读数z时,求解定位问题(估计x)和建图问题(估计y),这就把slam问题建模成了一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏的状态变量。
实践
安装linux操作系统
(这个网上有很多资料,等我下次重装系统时写一个如何安装与删除ubuntu系统并作基本的一些配置的博客吧)
Hello SLAM
打开终端使用vim编辑器创建文本HelloSLAM.cpp并输入以下代码
vim 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执行,然后就可以在终端中看见Hello SLAM了
使用cmake
g++可以编译任意的c++程序,但编译一个大型c++项目时,需要用g++命令一条一条编译,很繁琐。所以使用工程管理工具cmake处理工程文件代码之间的依赖关系,然后使用make命令调用g++来编译,这样就无需输入一大串g++命令。
先下载cmake:
sudo apt install cmake
验证安装:
cmake --version
出现版本号就安装成功了
用vim创建并编写一个文本:
vim CMakeLists.txt
内容为:
# 声明要求的 cmake 最低版本 cmake_minimum_required(VERSION 3.0.2) # 声明一个 cmake 工程 project(HelloSLAM) # 添加一个可执行程序 # 语法:add_executable( 程序名 源代码文件 ) add_executable(helloSLAM helloSLAM.cpp)
cmake . # 用cmake命令编译该工程
生成这些文件,其中最重要的是Makefile,然后用make命令根据这个文件编译整个工程
得到这个可执行文件
终端输入./helloSLAM执行就可以得到了
使用cmake和之前方法得到的结果相同,但过程从输入一串g++指令到维护若干个比较直观的CMakeLists.txt文件,明显降低维护整个工程的难度,如果想新增一个可执行文件,只需在CMakeLists.txt中添加一行add_executable命令即可,后续步骤是不变的。
在cmake过程中会生成很多中间文件,不太需要,所以我们希望把这些文件都放在一个中间目录中,编译成功后对其删除即可,所以更常见的做法是:分别输入以下代码
mkdir build cd build cmake .. make
cmake ..后是这样的
build里是这样的
使用库
在一个C++工程中,并不是所有代码都会编译成可执行文件。只有带有main函数的文件才会生成可执行程序。而另一些代码,我们只想把它们打包成一个东西,供其他程序调用。这个东西叫作库( Library )。
自己编写一个库,首先书写一个名叫libHelloSLAM.cpp的文件:
vim libHelloSLAM.cpp
内容如下:
//这是一个库文件 #include <iostream> using namespace std; void printHello() { cout << "Hello SLAM" << endl; }
这个库提供了一个printHello函数,调用此函数将输出一条Hello SLAM。但是它没有main函数,这意味着这个库中没有可执行文件。我们在CMakeLists.txt里加上如下内容:
add_library( hello libHelloSLAM.cpp)
这条命令告诉cmake,我们想把这个文件编译成一个叫作 "hello” 的库。(cmake需要额外学习)
然后再次编译cd build cmake .. make
在build文件夹中生成一个libhello.a文件,这就是得到的库。
在Linux中,库文件分成静态库和共享库两种。静态库以.a作为后缀名,共享库以.so结尾。
所有库都是一些函数打包后的集合,差别在于静态库每次被调用都会生成一个副本,而共享库则只有一个副本,更省空间。如果想生成共享库而不是静态库,只需使用以下语句即可。
add_library( hello_shared SHARED libHelloSLAM.cpp )
此时得到的文件就是libhello_shared.so
库文件是一个压缩包,里面有编译好的二进制函数。如果仅有.a或.so库文件,我们并不知道里面的函数到底是什么,调用的形式又是什么样的。为了让别人(或者自己)使用这个库,我们需要提供一个头文件,说明这些库里都有些什么。因此,对于库的使用者,只要拿到了头文件和库文件,就可以调用这个库。下面编写libhello的头文件。
vim libHelloSLAM.h
内容为:
#include "libHelloSLAM.h" // 使用 libHelloSLAM.h 中的 printHello() 函数 int main(int argc, char **argv) { printHello(); return 0; }
这样,根据这个文件和我们刚才编译得到的库文件,就可以使用printHello函数了。最后,我们写一个可执行程序来调用这个简单的函数:
vim useHello.cpp
内容为:
#include "libHelloSLAM.h" // 使用 libHelloSLAM.h 中的 printHello() 函数 int main(int argc, char **argv) { printHello(); return 0; }
然后,在 CMakeLists.txt 中添加一个可执行程序的生成命令链接到刚才使用的库上:
add_executable( useHello useHello.cpp )
target_link_libraries( useHello hello_shared )保存然后编译发现会生成可执行文件useHello文件,运行发现可以顺利使用库中的代码了。
# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 3.0.2)# 声明一个 cmake 工程
project(HelloSLAM)# 添加一个可执行程序
# 语法:add_executable( 程序名 源代码文件 )
add_executable(helloSLAM HelloSLAM.cpp)add_library( hello libHelloSLAM.cpp)
add_library( hello_shared SHARED libHelloSLAM.cpp )add_executable( useHello useHello.cpp )
target_link_libraries( useHello hello_shared )以上这些都是cmake的语法以及演示的一些功能,cmake我们需要继续学习。
IDE
前面的编程可以用vim来实现,但需求复杂,文件繁琐的情况下还是建议选择一个集成开发环境来进行开发,如Qt Creator,Clion......等笔者在此不做建议