SLAM系列(一)：入门介绍

磨蹭了好久终于开始写本行SLAM系列了。鄙人目前水平一般，但接下来的几年也都会在这个方向继续深入下去，所以打算尽我所能地写一个完整的SLAM从入门到精(fang)通(qi)的系列。文章的内容大概会分为三个大部分
1：2维平面的SLAM系统
2: 视觉SLAM
3：视觉惯性SLAM
每一个大部分都会分为代码和理论两个小部分，以帮助大家理解如何实现。
绝大多数的代码会在ROS的帮助下完成，这也方便大家在学习完ROS入门系列之后看看利用ROS写稍微大一点的程序是什么样子。
2维平面的SLAM系统主要参考<probabilistic robotics>
视觉SLAM会主要参考<multiple view geometry>, 一些关于非线性最小二乘优化的书籍以及<视觉SLAM十四讲>。代码参考ORBSLAM2等。视觉SLAM的教学代码有高翔博士的<一起来做RGB-D SLAM>，但是也有一些年份了，可以用更新的了。
视觉惯性SLAM主要参考一些论文了，比如和VINS-Fusion,OKVIS以及预积分相关的论文，我们会在以后说到。
可能好多人会直接想搞视觉SLAM部分而不想从2d部分学起，但是对于入门者来说，2d的理论和代码都会更简单一些，方便看懂为后面进阶铺路。
最后说一下作者是兴趣使然的更新，看心情更新，更新时间很长，也说不定断更。

基本要求

理论上你需要有一些基本的高数知识，比如怎么求积分怎么求偏导数什么的，基本线性代数知识，比如矩阵的初级操作，求逆什么的，以及概率论的基本知识，高斯分布，多变量高斯分布，全概率公式，贝叶斯法则什么的，编程上你需要对C++有基本的了解，知道并能编写简单的函数，类。不了解也不用担心，稍微深入的地方我都会讲到。

基本概念

SLAM全称Simultaneous Localization and Mapping，即同时定位与制图。定位的概念很好理解，如果一个机器人来到陌生的环境，它需要知道自己在哪儿，数学上来说就是知道自己的坐标，机器人如果在移动，就需要时刻的坐标更新。制图的意思是指对周围环境的了解，对周围环境的了解能帮助你更好地定位自己。制图分为稀疏制图(sparse mapping)和稠密建图(dense mapping)。稀疏制图表示你对周围的环境只有部分的了解，而稠密建图则表示你对周围的环境的每一个点都清楚。打个比方，你和父母一起出游，结果走散了，你给父母打电话，他们问你在哪儿，这时候你需要定位了(咳咳假设你不能直接发定位给他们)，你告诉他们:"我在xx路上，后面有一个xx楼，上面写着xxx。"你说的内容，提取了当前环境比较特征的部分，而这些比较特征的部分，这些就足够帮住确定你的位置了，这就属于稀疏建图。如果你告诉父母:"我在xx路上，前面是xx，后面是xx，左边是xx，右边是xx，斜前方是xx，斜后方是xx....."总之你把你周围的一切东西都描述了出来，这就就是稠密建图。稠密建图当然能帮助你定位，但很显然，如果你只是想单纯地定位，你给出的信息就太冗余了。所以SLAM中的定位只需要稀疏建图就可以了，你了解周围环境的一些特征，就足够能确定你的坐标了。
实际的稀疏建图，是提取环境中一些比较特征的点。如果你有一个相机，在获取一张照片之后，根据特定的算法，能把照片上一些比较有特色的点找出来，这些比较特色的点就能帮助你定位。总的来说SLAM里的地图不是大家想象中的百度地图高德地图google地图那样，而是一些从环境中提取出来的稀疏的比较有特点的点。
关于定位，还得介绍一下室内外的定位方法。在室外的话，利用GPS直接定位是很好的选择，不过普通民用GPS精度一般，误差可以有3米左右，也有高精度的GPS，误差可以到达厘米级，但是价格很贵。关于SLAM的室外应用，很多人肯定很感兴趣无人车方面的技术。无人车现在分级是L0~L5相信这个大家很多都听说过。其中有部分公司达到了L4级的无人车系统，这意味着在提供一定的辅助条件下，汽车可以自动驾驶。而这个辅助条件，比如说有预先制好的地图。再用前面那个你需要给父母提供你位置信息的例子来说，你的父母如果对当地很熟悉，也就是他们脑海中已经有地图了，你打电话的内容就可能更省略:"我在XX楼旁边“他们就能找到你了。回到无人车的例子，如果周围的环境已经在出发前就被知道了，无人车就能省掉一部分计算力并且定位的精度更高。如何预先知道环境呢？在现实中，可能专门有一辆车，利用激光雷达(lidar)在一条路上先开一次，获取到周围环境的3D点云，储存起来，这样地图就预先构建好了，能省掉很大的计算力(就好像之前的例子你给父母提供位置信息时能少说话)。所以说严格来讲目前大多数公司的无人车在室外并不是完全使用SLAM的技术，他们不仅有SLAM的影子(利用激光雷达或者相机等帮助定位并提取路人，车子等特征)，同时也借助了预先制好的地图和GPS等其他传感器辅助定位，以达到尽可能好的效果。虽然不是完完全全的自动化，但是L4级的无人驾驶还是很有意义的。比如说应用于公交车或者其他只在限定范围内运动的车辆，我们完全可以对周围的道路进行预先制图从而节省汽车在真正行驶过程中的计算量。甚至普通家用车，你如果大多数时候只在某个城市什么的行驶，也完全可以有专门的公司去预先制图。
而L5级的无人驾驶，其实才是真正的SLAM应用。L5级的无人驾驶不限环境和条件，意味没有预先制定的地图，我们必须真正地同时定位和制图。一般也需要GPS的辅助定位。当然无人驾驶除了SLAM还需要有很多其他技术，比如控制，决策，利用深度学习提取路人等信息什么的，要把他们融合起来非常复杂。L5级的无人驾驶据我目前所知没有公司达到，室外的SLAM应用也并没有那么成熟(等一下不要砸自己饭碗啊喂！！)，理论虽然很成熟了，但是面对千奇百怪的实际环境，那些很fancy的算法也是有些捉襟见肘。L3,L4级无人驾驶能满足很多无人驾驶的需求了，他们需要但不完全依赖于SLAM。至于L5级目前是一个目标，冲鸭！！
室内的定位就没有GPS的用武之地了，完全SLAM的地盘了。有名的应用领域比如扫地机器人什么的。

basic_idea.png

基本原理

这儿我用一个不算非常恰当但非常简单的例子帮助初学者对同时定位制图的计算原理有一个直观的了解。请参考上面basic_idea.png的图。
假设黑圆圈是一辆小车，方块A是环境中一个固定的点。小车从位置B运动到了位置C。小车在位置B时通过某种传感器测得A的位置是距离米，同时通过传感器和一些小的计算能知道AB和B运动方向之间的夹角即。运动到位置C后测得A的位置距离它是米，同样可以知道。这时候如果B点的坐标是，那三角形知道两边两角你能不能算第三边呢？
其实不仅我们可以通过测A和小车之间的距离最终了解的长度，我们当然还有传感器能测小车的速度之类的，假设小车从B运动到C只经历了很短的时间，我们测得小车在B点的速度延BC方向是，这很短的时间内我们可以假设小车的速度没有发生改变。那你能不能算边的长度呢？
可以说这两个问题都太简单了，但是如果你能完成你就已经完成SLAM即同时定位与制图了。A属于环境中的点，你能根据传感器确定它的位置，这就是制图；你根据小车在B的位置和一系列传感器数据以及小车的运动模型(它沿BC在极短的时间内坐匀速直线运动)推断出小车在C的位置，这就是定位。合起来就是Simultaneous Localization and Mapping。
说地这么简单，为什么实际想实现一个SLAM系统却没那么容易呢？我们来看看如果面临现实情况会有哪些问题。
Question1:我们说通过测量A相对于B，C的位置构成一个三角形能辅助我们算出，通过测得小车在B的速度以及假设它匀速直线运动我们也能算出。如果传感器完美测量没有任何误差，运动模型也完美，这两种方式算出的应该一样。但当然不会完美。假设测得的长度和真实值差0.01m,测得的长度和真实值差0.01m，那算出的长度就会和真实值有差距，我们把这个差距记为。如果小车在B测得的速度和真实的速度有0.001m/s的差距，小车从B到C也并不是完美的匀速直线运动，那算出的长度也会和真实值有差距，记为。传感器不同，肯定会不一样。那么我们该使用哪个值？
Question2:我们非常完美地假设能直接测得的长度，视觉SLAM中我们有相机作为传感器，普通的相机拍出的是一张图片，即二维平面，A位于这个平面中，我们怎么能确定这个平面中一个点到自己的具体距离？
Question3:假设我们已经能在小车在B点时算出A的位置，小车运动到C时算出A距离C的位置，但我怎么知道这两个A是一个A??有点绕口，这么说，小车在B点拍下了当前环境的照片，有一棵树，在C点拍下了照片，有同样一颗树，这两张照片储存在电脑中，但是电脑不会知道这是同一棵树。图片作为数字矩阵储存在电脑中，这两棵树在电脑里面都只是一堆不相关数字而已。
Question4:环境中不仅有点A，还有点M,N,L....，假设每一个点我们都能测出到小车的距离，但是由于传感器的测量有误差，并且测出的误差各不相同，有的差0.01m有的差0.0095米，有的差0.011米，环境中的每一个点都和B,C形成了一个三角形，每一个三角形都能算出的长度且由于误差算出的各不相同，那么和Question1类似的问题又来了，我们使用哪一个 ？
Question5:由于我们前面的说的测量的误差，无论我们选哪个，或者用某种方式把这些整合起来算一个新的（最简单的，加起来求个平均什么的），都不会是真实的值，仍然会和真实的值有一定误差。当我们计算出的C的位置和实际位置差0.01米，之后小车从C运动到D又算差0.011米，之后小车从D运动到E又差0.0095米...每一个是0.1s的话，如此累积，只需要10s小车的位置误差就能达到米级！误差的累积是不可避免的。面对这种情况，我们又该怎么办？
等等还有一系列的问题，其实最主要的还是来自于测量的不准确性和模型的不准确性(就像小车不是完美匀速直线运动)。现实的SLAM如何处理这些问题，我们会在以后的讲解中一一解答

符号约定

在理论讲解时，会涉及到很多符号描述。符号这个东西，有些统一了有些没统一还真是麻烦。我们讲解的符号约定如下，普通没加粗的希腊字母表示一个数字，或者英语数学里常说scalar，比如；加粗的小写字符表示向量；大写加粗表示矩阵；空心的加粗大写表示集合，比如实数集。