SLAM概念

SLAM (Simultaneous Localization and Mapping)：同步定位与建图。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。

假设你是《鲁滨孙漂流记》中的主人公，你漂流到一个无人的荒岛上时，你需要迅速熟悉周围环境，寻找食物和水源，躲避野兽，那么你就需要完成以下事情：

1. 用眼睛观察周围的地标，比如一座最高的山峰、一颗造型奇特的石头、一棵参天大树等等，并且记住这些地标的特征——特征提取。

2. 根据眼睛所观察到的环境信息，在脑海里构建一个包含这些地标的地图，对它们的方位有一定的掌握——三维建图。

3. 你开始沿着一条小路往前走，本来在你前方的一棵树现在变成了在你左侧，虽然你和树的相对位置变化了，但你知道在你左侧的这棵树就是刚才在你前方的那棵——特征关联。

4. 通过你和树相对位置的变化，你可以得到自己的运动轨迹、方向、速度、当前运动状态——运动估计。

5. 在你前进的过程中，你会遇到新的地标，比如一条小河、一个洞穴等，根据获取到的它们的方位信息，校正自己脑海里的地图模型——状态更新。

6. 当你无意中走了很长一段路以后，与脑海中的地标进行匹配，查看自己是否走回了原路——回环检测。

上述你做的这些事情，就是视觉SLAM的几个重要组成部分。

特征提取

基于特征的SLAM算法是通过两帧间相关联的特征来计算两帧间相机的相对位姿关系的，因此特征提取的好坏直接影响SLAM算法的表现。一个好的特征应该具有良好的光照不变性和几何不变性，即无论在何种光照条件和几何投影下，同一特征不能有较大变化，否则不同帧图相间的特征无法匹配会导致SLAM运动估计失败。

常用的特征包括点特征、直线特征、边缘特征等。由于直线特征容易被遮挡，而边缘直线的起始点可能不存在，因此绝大多数研究者使用点特征，而将直线特征在结构化环境中作为补充特征。常用的点特征一般分为角点特征和块特征两类。

角点特征，即两个或多个区域的交点，检测速度快、易于计算、易于定位，但差异性较小。

块特征一般通过颜色、强度、纹理等信息来划分区域，特征好、能稳定地检测出来，但检测速度慢。

一般而言，人工设计的特征点需要拥有如下性质：

1． 可重复性：相同的特征可以在不同的图像中找到。

2． 可区别性：不同的特征有不同的表达。

3． 高效率：统一图像中，特征点的数量应远小于像素的数量。

4． 本地性：特征仅与一小片图像区域相关。

特征关联

两张图像匹配最简单的方法就是遍历图像中所有的特征点，计算距离，距离最小的作为匹配点。为了防止出现一对多的匹配结果，使用双向匹配的方法来确定最终结果。但当特种点数量较多时，计算量会非常大，无法实用。

多分辨率法：先在低分辨率下进行搜索，找到近似的最优匹配块之后，再切换至高分辨率下进行修正。运算复杂度较高。

通过在第二张图像上期望的区域中来搜索可能的匹配点来提高匹配效率，期望区域可以通过三维点和建立运动模型的方式来得到，车辆的运动模型可以通过IMU、轮速计、GPS等获取。

运动估计

SLAM问题的一项核心任务就是对相邻两帧图像的运动变化做出估计，从而得到整体的运动轨迹以及当前运动状态。在得到对应的特征后，求出两者之间的空间位置的相对偏移量，得到的相对偏移量就是通常所指的运动矢量，得到运动矢量的过程被称为运动估计。

根据使用的匹配特征点维度的不同，大体上有三种估计方法。

1． 待计算的两帧图像间的特征点都用二维图像坐标进行表示。

2． 待计算的两帧图像间的特征点都用三维图像坐标进行表示。

3． 待计算的两帧图像间，前一张用三维坐标表示，后一张用二维坐标表示。

因为前一帧图像的特征点都已经被三角化，即已经知道了这些点的3D位置。那么新的帧到来后，通过图像匹配就可以得到与那些3D点相对应的2D点，再根据这些3D-2D的对应关系，利用PnP算法解出当前帧的相机位姿。PnP问题有多种求解方法，包括P3P、直接线性变换（DLT）、EPnP（Efficient PnP）、UPnP等等，而且它们在OpenCV中都有提供。因为三维坐标需要根据前面多张照片及运动模型来获取，不可避免地会引入额外的误差。

后端优化

前端视觉里程计能给出一个短时间内的轨迹和地图，但不可避免地误差累积，这个地图在长时间内是不准确的。要想构建一个尺度，规模更大的优化问题，以考虑长时间内的最优轨迹和地图，这时后端优化就出现了。常用优化算法对比如下：

算法

缺点

优点

KF/EKF

假设噪声为高斯分布，在高维状态空间时计算效率较低，不适合大规模场景的地图构建。

原理简单，小场景下收敛性好。

PF

会不可避免出现粒子退化现象，每个粒子需包含整张地图信息导致不适合大场景地图构建。

将运动方程表示为一组粒子，通过蒙特卡罗方法求解，能够处理非线性非高斯噪声。

EM

计算量大，不能用于大规模场景。

有效解决了数据关联问题。

图优化

对闭环检测算法的要求很严格。

出现多种图优化框架，能够有效解决滤波器算法的缺陷，能用于大规模场景的地图创建。

回环检测

回环检测，又称闭环检测，是指机器人识别曾到达某场景，使得地图闭环的能力。说得简单点，就是机器人在建图的时候能意识到某个地方是“我”曾经来过的，然后把此刻生成的地图与刚刚生成的地图做匹配。

回环检测之所以能成为一个难点，是因为：如果回环检测成功，可以显著地减小累积误差，帮助机器人更精准、快速的进行避障导航工作。而错误的检测结果可能使地图变得很糟糕。因此，回环检测在大面积、大场景的地图构建上是非常有必要的 。

常用的回环检测方法有图优化、词袋模型、相似度计算、决策树、SVM等。