重要说明：本文从网上资料整理而来，仅记录博主学习相关知识点的过程，侵删。

一、参考资料

SLAM的一点认识
视觉SLAM方案分类及选型调研
SLAM技术框架介绍
视觉SLAM漫谈

参考文献

[1] Relative Bundle Adjustment

二、知识储备

1. 视觉词袋

视觉词袋（bags-of-visual-words）

2. PnP问题

计算相邻图像帧间的转换关系，成为PnP问题。

3. Landmarks路标

路标点是那些容易被再次检测出的特征点，且明显跟环境不一样的特征。决定让机器人识别的路标点应该足够多，否则机器人会走丢。路标点应该是静止的，选择人为路标点是很糟糕的想法。

适合作为路标点的关键点，应该具有以下特性：

• 容易被再次观测到；
• 明显区别于其他的关键点；
• 数量要多；
• 需要是静止的。

路标点提取

使用激光扫描仪的基本路标点提取算法：Spikes和RANSAC。

4. 码盘

从码盘中获得相邻图像帧间的位移差。

5. 视觉vSLAM学习路径

三、视觉vSLAM相关介绍

1. 视觉vSLAM的概念

最早的SLAM雏形是在军事领域的应用，例如：核潜艇的海底定位。SLAM技术发展到如今几十年，目前以激光雷达为主传感器的SLAM技术比较稳定、可靠，仍然是主流的技术方案。但随着最近几年计算机视觉技术的快速发展，SLAM技术越来越多的应用于家用机器人、无人机、AR设备，基于视觉的Visual SLAM（简称VLSAM）逐渐开始崭露头角。

与激光传感器相比，视觉传感器具有成本低、保留环境语义信息的优点，能够与深度学习进行大量融合。

视觉vSLAM是基于视觉传感器的SLAM系统，它利用相机获取环境图像信息，利用多视图几何算法实现定位和环境建图。视觉vSLAM技术广泛应用于无人驾驶、智能机器人、元宇宙、游戏等领域。

2. 视觉vSLAM的难点

视觉vSLAM是通过视觉传感器获取灰度图像或者彩色图像，对于图像中的一个像素，它只能告诉我们在某个方向有障碍点、障碍点周围的表观（local appearance）如何，但它不能告诉我们这个障碍点的距离。要想计算该点的距离，需要把相机挪动一个位置再对它观察一次，然后通过特征提取、特征点匹配，再按照三角测量的原理推算出障碍物的距离。

2.1 速度与性能（精度）问题

原理上很清晰，但实际做起来并不简单。首先需要在两幅图像中寻找点的对应，这涉及到特征点的提取和匹配、或者准稠密点之间的匹配。计算机视觉发展到今天，其实还不存在性能和速度上很好满足视觉vSLAM的特征提取和特征匹配的算法。常见的特征点提取算法，性能上大致可以认为：SIFT>SURF>ORB>FAST，效率上可以认为：FAST>ORB>SURF>SIFT（大于号左边代表更优）。性能主要包括：匹配精度、特征点的数量和空间分布等。为了在性能和效率上取得折中，通常采用FAST或者ORB，只能舍弃性能更好的SIFT、SURF等。

2.2 非线性优化（图优化）问题

其实，匹配点的图像坐标与空间坐标之间的关系是非线性的，例如2D-2D点的对应满足对极几何，2D-3D点的对应满足PnP约束。这些匹配数量较多，前后两帧图像中一般有几十至数百的匹配。这些匹配会引入众多约束关系，使得待估计变量的关系错综复杂。为了得到一个较优的估计，通常需要建立优化问题，整体优化多个变量。说起来这无非是一个非线性最小二乘优化问题，但实现起来并不简单，因为存在非线性约束、约束数量很多、存在误差和野值点，并且要将计算时间控制在允许范围。目前广泛采用关键帧技术，并且通过很多方法来控制问题规模、保持问题的稀疏性等。

如下图所示，圆饼（节点）代表优化的变量（相机位姿、特征点的空间坐标），杆子（边）代表约束（对极几何、PnP等）。

2.3 总结

前面分析了视觉vSLAM的两个困难。前者导致了前端的特征跟踪不易，后者导致了后端的优化不易。想做出一个高效率、鲁棒性的视觉vSLAM系统还是一个非常有挑战的任务。效率方面，SLAM必须是实时运行的。如果不能做到实时，就不能称作SLAM。不考虑实时性，采用从运动恢复结构（structure-from-motion，以下简称SfM）效果会更好。鲁棒性方面，一个脆弱的系统会导致用户体验很差，功能有限。

使用SfM对玲珑塔进行三维重建，其效果如下图所示：

3. 曾经的视觉vSLAM

伦敦帝国学院的 Andrew Davison 教授，曾在 2007 年的 BMVC（英国机器视觉大会）上给出了一个视觉 SLAM 教程。让人惊讶的是，和主要的视觉大会上其它机器学习技术的纷繁成果相比，SLAM 的变化真是非常之少。过去八年里，目标识别已经经历了两三次小型变革，而今天的 SLAM 系统和其八年前的样子看起来并没有多大不同。

了解SLAM的进展最好方法是看最成功和最让人难忘的系统。在 Davidson 的研讨会介绍演讲中，他讨论了一些过去 10-15 年里科研界所打造的典范系统：

• MonoSLAM；
• PTAM；
• FAB-MAP；
• DTAM；
• KinectFusion；

4. 视觉vSLAM与深度学习

视觉vSLAM算法可以实时构建世界的3D地图，并同时追踪摄像头的位置和方向，该摄像头一般安装在机器人上，或者安装在手持式或AR设备的头盔上。SLAM是卷积神经网络和深度学习的补充：SLAM关注几何问题，而深度学习关注感知、识别问题。如果你想要一个能走到你的冰箱面前，而不会撞到墙壁的机器人，那就使用SLAM；如果你想要一个能识别冰箱中物品的机器人，那就使用卷积神经网络。

四、视觉vSLAM分类

视觉vSLAM方案，从不同角度对其进行分类：

• 按照传感器的不同，可分为：单目（Monocular）相机、双目(（Stereo）相机、深度（RGBD）相机、与IMU结合，除此之外，还有特殊的环视相机、鱼眼相机等；
• 按照前端视觉里程计方法的不同，可分为：直接法、特征点法；
• 按照后端优化方法的不同，可分为：滤波法、非线性优化法；
• 按照生成地图形式的不同，可分为：稀疏地图、稠密地图。
  

本章节主要以传感器的不同对视觉vSLAM方案进行介绍。单目SLAM仅使用单个相机，而非单目SLAM通常使用一个预校准的固定基线的立体相机套件。

0. 特征点法vs直接法

特征点法是视觉vSLAM的主流方法，根据提取、匹配特征点来估计相机运动，优化的是重投影误差，对光照、动态物体不敏感，常见的开源方案有：ORB-SLAM系列算法。

直接法是根据像素的灰度信息，估计相机的运动，可以不用计算关键点和描述子，优化的是光度误差。根据使用像素的数量，可以分为稀疏、半稠密和稠密三种类型。常见的开源方案有：LSD-SLAM、DSO等。

1. 单目SLAM

单目SLAM初始化：单目相机在初始化阶段进行一段位移，且确定该段位移的实际大小作为参考。

单目SLAM，只使用一个相机采集信息，在尺度完成初始化的情况下，能够完成连续图像间相机位姿的计算与建图。

单目SLAM，即MonoSLAM，主要通过使用一个相机并结合IMU等传感器进行机器人位姿估计的方法。由于它使用的传感器数量最少，因此成本最低，最为轻便。但由于只有单目信息，往往会缺失一些物理尺度，并且对于原地旋转无法适用，因此特别需要IMU辅助SLAM算法。

2003 年，伦敦帝国学院的 Andrew Davison 教授提出了知名的 MonoSLAM 系统，他是第一个展示如何在单个摄像头上构建SLAM系统的人，而那时候其他所有人都还认为打造SLAM系统需要一个立体的双目摄像头套件。

最近，他的研究成果已经对戴森（Dyson）等公司的发展轨迹和他们的机器人系统的能力产生了影响，例如全新的Dyson360。

1.1 单目SLAM优缺点

单目视觉vSLAM算法存在一些本身框架无法克服的缺陷，首先是尺度的问题，单目SLAM处理的图像帧丢失了环境的深度信息，即使通过对极几何约束和三角化恢复空间坐标点的三维信息，但恢复的深度量程是任意的，并不是实际的物理尺寸。也就是说，单目SLAM估计出的运动轨迹即使形状吻合，但是尺寸大小不是实际轨迹的尺寸。由于基于视觉特征点进行三角化的精度和帧间位移有关系，党相机进行近似旋转运动时，三角化算法会退化，导致特征点跟踪丢失。同时视觉vSLAM一般采用第一帧作为世界坐标系，这样估计出的位姿是相对于第一帧图像的位姿，而不是相对于地球水平面（世界坐标系）的位姿，后者是导航中真正需要的位姿。换而言之，单目SLAM估计的位姿不能和重力方向对齐，而重力向量构建了视觉坐标系和世界坐标系的联系。

单目SLAM优缺点总结如下：

优点

• 成本低；
• 搭建简单；

缺点：

• 初始化问题：需要专门初始化；
• 尺度不确定性问题：尺度不确定性问题：尺度不确定性问题是指，在单张图片里，无法确定一个物体的真实大小。它可能是一个很大但很远的物体，也可能是一个很小但很近的物体。；
• 深度计算存在缺陷。图像的深度信息只能通过三角测量计算得到，对环境适应性差。
  • 3D假图问题；
  • 机器学习的样本问题；
• 建图精度不足：在相机位移较小时，计算误差较大，建图精度不高。

单目相机只能获得相对深度，并且无法依靠一张图像获得相对深度（图像中物体离自己的相对距离）。换句话说，只有在相机运动之后，深度信息才能收敛，如果相机不运动，就无法获得像素的深度。同时，相机运动还不能纯粹的旋转，这给单目SLAM的应用带来一些麻烦。所以，国内很少有移动机器人厂商会使用单目相机来进行地图构建。

1.2 经典的单目SLAM系统

1. PTAM(Parallel Tracking And Mapping)：2007年 很牛逼的SLAM项目，是第一个使用BA完成实时SLAM的系统，论文中提出的关键帧以及多线程处理思想成为之后视觉vSLAM标配。但其由于缺乏回环检测和重定位等功能，只能作用于小规模的场景，且稳定性也不是很高；
2. DTAM(Dense tracking and mapping in real-time)：2011年，Direct SLAM方法的鼻祖；
3. LSD-SLAM(Large-Scale Direct Monocular SLAM)：2014年一个半稠密SLAM系统；
4. ORB-SLAM：2015年一个比较完整的基于特征点的SLAM系统。

2. 双目SLAM

双目SLAM，使用双目相机采集信息，通过立体视觉原理，计算得到像素深度。具体来说，双目SLAM通过同时曝光左右两个相机，获取同一时刻的图像，并基于两个相机的物理距离实现环境中特征点的空间定位，进而估计自身的位姿状态。

由于物理结构上的先天优势，可以获得绝对深度，并且可以恢复出较为稠密的深度点云数据。

2.1 双目方案

2.2 双目SLAM优缺点

优点

• 不需要专门初始化；
• 能够计算绝对深度；
• 能够用于室内和室外，对环境适应性高于单目；

劣势

• 配置复杂：相机标定较为复杂；
• 计算量大：视差计算比较耗资源，需要GPU/FPGA 或专门的ASIC芯片加速。产品化时，需要将算法移植到嵌入式芯片中；
• 需要考虑两个摄像头的图像的时间同步问题。

双目立体视觉，既可以在运动时估计深度，亦可在静止时估计深度，消除了单目视觉的许多麻烦。不过，双目或多目相机配置与标定较为复杂，其深度量程也受到双目的基线与分辨率限制。

3. RGB-D SLAM

RGB-D相机，是能够同时输出RGB图像和对应深度图的相机，其测量像素深度不再通过耗时的被动双目匹配算法，而是通过结构光或TOF原理，直接测出图像中各像素离相机的距离。因此，RGB-D相机相比传统相机，能够提供更丰富的信息，也不必像单目或双目那样费时费力地计算深度，可以看作是计算量减少的双目SLAM。但是，RGBD相机普遍在室外表现效果不佳，更多用于室内环境。

3.1 主流的RGB-D相机

• ZED系列：ZED2、ZEDmini、ZED；
• Realsense系列： D415、 D435、 D435i、 D455、ZR300；

3.2 RGB-D SLAM优缺点

优点

• 主动测量深度；
• 重建效果好；

缺点

• 测量范围小；
• 受日光干扰，室外效果差；
• 受材质干扰；

3.3 经典的RGB-D SLAM系统有

1. KinectFusion 2011；
2. RGBD-SLAM2 2014；
3. ElasticFusion 2015；

4. 单目/双目+IMU的SLAM

在单目/双目+IMU的SLAM方案中，其中IMU主要起到的作用包括：

1. 解决单目SLAM的初始化尺度问题；
2. 提供较好的初始位姿；
3. 提供重力方向；
4. 提供一个时间误差项以供优化。

理论上来说，IMU提供了冗余的运动信息，通过数据融合可以得到更加精确的运动估计。

五、视觉vSLAM数学模型

1. 运动概率模型

运动模型，是指机器人从一个地方运动到另外一个地方，位姿变化与运动控制量之间的关系。由于控制量作用在机器人上，使机器人的位姿变化的过程是一个存在误差的不确定过程，所以控制量作用后产生的结果需要用一个概率分布来描述，这个概率分布即为运动概率模型。

根据运动控制量的不同，可以分为：速度运动模型和里程计运动模型。速度运动模型中，控制量是线速度和角速度；里程计运动模型中，控制量是机器人的前后时刻的相对位姿。速度运动模型中，线速度和角速度均会受到机器人底盘构造的约束。实验表明，虽然里程计运动模型存在误差，但比速度运动模型有更高精度。

2. 测量概率模型

同样的道理，机器人上的传感器在测量过程中存在误差，所以测量结果需要用一个概率分布来描述，这个概率分布即为测量概率模型。

3. 视觉vSLAM通俗解释

将连续时间的运动转换成离散时刻 $t=1,2,...,n$ 的运动，在时刻 $t_i$，用 $x_i$ 表示相机的位置，则 $x=x_1,x_2,...,x_K$ 表示相机的运动轨迹。

假设地图由许多地标组成，每个时刻相机可以观测到部分路标，用 $y=y_1,y_x,...,y_N$ 表示地图中观测到的第 $i$ 个路标。

3.1 运动方程

$什么是运动？$相机从 $k-1$ 时刻到 $k$ 时刻的自身位置发生变化。用抽象的数学模型表示为：
$$x_k=f(x_{k-1},u_k,...,w_k)$$
其中， $u_k$ 表示相机获取的环境图像数据，$w_k$ 表示该过程中的噪声数据。该方程称为运动方程。

3.2 观测方程

$什么是观测？$ $k$ 时刻相机的位置为 $x_k$，观测到某一个路标 $y_i$，产生了一个观测 $z_{i,j}$。用抽象的数学模型表示为：
$$z_{i,k}=h(y_i,x_k,...,v_{k,j})$$
其中，$v_{k,j}$ 表示该过程中的噪声数据。该方程称为观测方程。

4. 视觉vSLAM数学推导

我们用抽象的数学表达式来描述视觉vSLAM数学模型。设：
$$\text{Location:}{x}_p^i,i=1,...,n$$

$$\mathrm{Landmarks}:x_L^i,i=1,...,m$$

$$\text{Motion:}{x}_p^{i+1}=f(x_p^i,u_i)+w_i$$

$$\text{Observations:}{z}_{i,j}=h(x_p^i,x_L^j)+{\nu }_{i,j}$$

$Location$ 方程中，$x_p$ 是机器人的位置，假定由n帧组成。如果做2D SLAM，那么机器人的位置就是（x，y）加一个转角 $\theta$ ；如果做3D SLAM，那么机器人的位置就是 （x，y，z）加一个四元数姿态（或者rpy姿态），这个过程叫做参数化（Parameterization）。

$Landmarks$ 方程中，$x_L$ 是路标，描述的是在视觉里程计过程中提取的特征点。

Motion 方程，称为运动方程，描述机器人怎么运动。$u$ 是机器人的输入，$w$ 是噪声项。最简单的形式，利用码盘获取相邻图像帧间的位移差，上一帧与u相加，即为该方程所得。运动方程描述的是运动模型。

$Observations$ 方程，称为观测方程，描述那些路标是怎么来的。在第i帧看到第j帧的路标，产生了一个观测值，就是图像中的横纵坐标。$v$ 是噪声项。观测方程描述的是观测模型。

在上面的模型中，我们知道的是运动信息u以及观测值z。用示意图表示如下：

根据已知的运动信息u和观测值z，求解所有的 $X_p$ 和 $X_L$ ，这就是SLAM理论。从SLAM诞生开始，科学家们就一直在解决这个问题。

六、视觉vSLAM经典框架

1. vSLAM三个线程

自PTAM算法以来，视觉vSLAM框架基本趋于固定。通常包括3个线程：前端tracking线程，后端优化线程，回环检测（loop closure）线程。

1.1 前端tracking线程

前端tracking线程，主要涉及：

1. 特征点的提取，特征点的匹配；
2. 多视图几何的知识，包括对极几何、PnP、刚体运动、李代数等。

1.2 后端优化线程

后端优化线程，主要涉及：

1. 非线性最小二乘优化，属于数值优化的内容；
2. 滤波算法、状态估计。

1.3 回环检测线程

回环检测线程，主要涉及地点识别，本质上是图像检索问题。

2. 视觉vSLAM框架简介

视觉vSLAM的技术框架，主要包括：传感器数据采集、视觉里程计、后端优化、建图（Mapping）、回环检测（Loop closure detection）。如下图所示：

3. 传感器数据

在视觉vSLAM中，主要为相机图像信息的读取和预处理，如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。

4. 视觉里程计（前端）

视觉里程计（Visual Odometry，VO），又称为前端（Front End），根据相邻帧图像，估计帧间相机的运动，以及局部地图。

视觉里程计是利用一个图像序列或者一个视频流，计算摄像机的方向和位置的过程。具体来说，通过将相邻帧的运动轨迹串起来，构成相机载体（如机器人）的运动轨迹，通过对极几何估计相机位姿信息（相机外参）；然后，根据相机位姿信息，计算各像素的空间点的位置，得到了像素的深度信息，并构建成地图。

视觉里程计主要涉及计算机视觉相关的算法，典型的做法一般是：首先提取每帧图像特征点，对相邻帧进行特征点粗匹配，然后利用RANSAC（随机抽样一致）算法去除不合理的匹配对，得到位置和姿态信息。整个过程涉及的技术有：图像获取、畸变矫正、特征提取、特征匹配、对极几何、PnP、刚体运动、李代数等多视图几何知识。

4.1 特征提取

4.2 特征点匹配

上面的例子可以看出，我们找到了一些匹配，但其中有些是对的，有些是错误的。这是因为图像中存在周期性出现的纹理（黑白块），容易搞错。为了解决特征点匹配误匹配的问题问题，需要进行后端优化。

5. 后端优化

5.1 引言

如果视觉里程计模块估计的相机的外参（旋转矩阵R和平移向量t）都正确的话，就可以得到完美的定位和建图。但在实际实验中，我们得到的数据往往有很多噪声，且由于传感器的精度、错误的匹配等，对结果造成误差。

前面介绍的视觉里程计，只计算相邻帧的运动，进行局部估计，这不可避免的出现累积误差。这是因为每次估计两图像帧间的运动都是有一定误差的，经过相邻帧多次传递，前面的误差会逐渐累积，轨迹漂移（drift）的越来越严重。解决轨迹漂移的方法有两个：后端优化和回环检测。

轨迹漂移现象

5.2 后端优化的简介

后端接收不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致性的轨迹和地图。由于接在VO之后，又称为后端（Back End）。

上面提到，仅用帧间匹配最大的问题在于误差累积，后端优化可以有效减少累积误差。具体来说，后端优化是对前端（视觉里程计）的结果进行优化，得到最优的位姿估计。

5.3 后端优化理论模型

5.4 后端优化方法

从上面章节介绍的后端优化理论模型中可知，为了求解所有的 $X_p$ 和 $X_L$ ，最初科学家们采用卡尔曼滤波，设计了运动方程和观测方程，搭建成上面描述的后端优化理论模型。直到21世纪初，卡尔曼滤波仍然在SLAM系统占据最主要的地位，Davison经典的单目SLAM就是用EKF做的。但后来，出现了基于图优化的SLAM方法，渐渐有取而代之的地位。

5.4.1 基于滤波理论的优化方法

基于滤波理论的优化，主要有EKF（扩展卡尔曼滤波）、PF、RBPF、UKF等方法。其中EKF是早期主流的方法，它的思路是将状态估计模型线性化，并用高斯分布近似其噪声，然后按照卡尔曼滤波进行预测和更新。但实际上，在大部分情况下，这种高斯分布的噪声是不成立的，此外线性化过程中丢失了高阶项。从$Landmarks$ 方程中中可知，由于卡尔曼滤波方法存储n个路标，要消耗n平方的空间，导致计算量大。

5.4.2 图优化（非线性优化）方法

图优化（Graph-based SLAM），是非线性优化，它的基本思想是：将优化的变量作为图的节点，误差项作为图的边，在给定初值后，就可以迭代优化更新。由于图优化的稀疏性，可以在保证精度的同时，降低计算量，效果也较好。图优化一般使用g2o求解器，进行图优化计算。

图优化方法把SLAM问题做成了一个优化问题。为了求解机器人的位置和路标位置，我们可以先做猜测，猜测机器人的大概位置；然后将猜测值与运动模型/观测模型给出的值进行比较，可以计算出误差。用数学公式可以表达为：

通俗一点讲，我猜机器人第一帧在（0, 0, 0），第二帧在（0, 0, 1）。但是 $u_1$ 告诉机器人往z方向（前方）走了0.9米，那么运动方程就出现了0.1m的误差。同时，第一帧机器人发现了路标1，他在该机器人图像的正中间；第二帧却发现它在中间偏右的位置。这时，我们猜测机器人只是往前走，这个猜测也是存在误差的，至于这个误差是多少，可以根据观测方程算出来。

将这些误差平方后加起来，得到平方误差和，即为优化问题的目标函数，数学公式表达为：
$$\min \phi =\sum _i(e_p^i{)}^2+\sum _{i,j}(e_L^{i,j}{)}^2$$
其中，优化变量为 $X_p$ 和 $X_L$ 。改变优化变量，误差平方和（目标函数）会相应的变大或变小，我们用数值方法求它们的梯度和二阶梯度矩阵，然后用梯度下降法求最优值。
$$J=\frac{\nabla \phi }{\nabla x},H=\frac{{\nabla }^2\phi }{\nabla x\nabla x^T}$$
注意到，一次机器人SLAM过程中，往往会有成千上万帧，而每一帧都有几百个关键点，一乘就有几百万个优化变量。这个规模的优化问题，用 Graph-based SLAM是无法计算的。但在21世纪06、07年之后，研究人员发现，这个问题规模没有想象的那么大。上面的 $J$ 和 $H$ 两个矩阵是稀疏矩阵，可以用稀疏代数的方法来求解问题。稀疏的原因是，每个路标往往不可能出现在所有运动过程中，通常只出现在一小部分图像里。这是这个稀疏性，使得优化思路成为了现实。

对于优化问题的稀疏性，通常用Graph图来表达这个问题。所谓图，就是由节点和边组成，写成G={V, E}，V是优化变量节点，E是运动/观测方程的约束。以文献 [1] 中提供图结构为例，如下图所示：

上图中，$p$ 是机器人位置，$l$ 是路标， $z$ 是观测，$t$ 是位移。其中，$p$ 和 $l$ 是优化变量， $z$ 和 $t$ 是优化的约束。因为每个路标不可能出现在每一帧中，所以这个图是稀疏的。

5.4.3 总结

后端优化涉及到的数学知识比较多，具有较高的难度。总的来说，从状态估计的角度来说，SLAM是一个非线性非高斯分布的系统。因此，传统的滤波理论已经逐渐被抛弃，而图优化已经成为主流方法。

5.5 后端优化的库

三大后端优化方法(Ceres&G2O&&GTSAM)

5.5.1 ceres

安装ceres非线性优化库

该部分内容待补充…

5.5.2 g2o

该部分内容待补充…

TODO

5.5.3 gtsam

该部分内容待补充…

6. 回环检测

6.1 引言

后端优化可能得到一个比较优的解，但当运动回到某一个之前去过的地方，如果能认出这个地方，并找到那个时候的关键帧进行比较，就可以得到比单用后端优化更准确更高效的结果。回环检测就是要解决这个问题。

6.2 回环检测的简介

回环检测（Loop closure detection），又称为闭环检测，用来判断机器人是否到达曾经去过的位置，如果检测到回环，则会把信息提供给后端进行处理。回环检测的主要目的是：让机器人能够认识自己曾经去过的地方，从而解决位置随时间漂移的问题。

回环检测，一般通过判断图像之间的相似性完成，这和我们人类用眼睛来判断两个相同地点是一样的道理。因为图像信息丰富，因此视觉vSLAM在回环检测中具有很大的优势。具体来说，回环检测有两种方式：

1. 一种是根据估计出来的机器人位置，判断是否与以前某个位置邻近；
2. 另外一种是根据图像外观，看它是否和以前关键帧相似。

目前主流方法是第二种，因为很多科学家认为，第一种依靠有噪声的位置来减少位置的噪声，有点循环论证的意思。第二种方法，本质上是一个非监督的模式识别问题。比较常用的方法是使用 Bag-of-Words（BOW），ORB-SLAM就是使用这个方法来进行回环检测的。

当回环检测成功之后，会建立现在的图像与过去曾经见过图像的对应关系，后端优化算法可以根据这些信息来重新调整运动轨迹和地图，从而最大限度地消除累积误差。

7. 建图（Mapping）

建图是指根据估计的轨迹，建立与任务要求对应的地图。SLAM根据不同的传感器类型和应用需求的不同，建立不同的地图。常见的有2D栅栏地图、2D拓扑地图、3D点云地图等。

7.1 建图的必要性

一般在机器人场景中，建图是一个不可或缺的功能。因为前端视觉里程计和后端优化得到的运动信息始终包含累积误差，该累积误差在SLAM中只能通过回环检测或者与事先建立好的具有较高精度的全局地图匹配来消除。但是机器人在实际运动中，不能保证全局路径一定会重叠，也就是说计算SLAM过程中，可能出现不存在回环的问题。此时，累积误差只能通过与全局地图匹配来消除，因此SLAM建图功能是不可或缺的。这一点在开源项目VINS（香港科技大学团队基于单目+IMU开发的开源SLAM方案）中有体现。在回环检测前，SLAM计算得到的位姿和真实位姿之间已经产生了很大偏差，该偏差只有在相机回到曾经去过的位置且SLAM成功执行了回环检测之后，才能得到消除。

7.2 2D栅栏地图

比如扫地机器人，它只需要知道房屋内部的简单二维地图，不需要知道房屋到底有多高；它只需要知道哪里可以通过，哪里是障碍物，而不需要知道这个障碍物到底是什么，长什么样子。目前，大部分具有SLAM功能的扫地机器人几乎都是采用廉价的消费级激光雷达方案，很少采用视觉vSLAM方案，且视觉vSLAM不够稳定。

7.3 2D拓扑地图

2D拓扑地图更强调地图元素之间的连通关系，而对精确的位置要求不高，去掉了大量地图的细节，是一种非常紧凑的地图表达方式。如下图所示：

7.4 3D点云地图

3D点云地图在视觉vSLAM中用的比较多，主要用于真实场景的视觉重建，重建的地图非常直观漂亮。但是点云地图通常规模很大，比如一张VGA分辨率（640x480）的点云图像，就会产生30万个空间点，这回占据非常大的存储空间，而且存在很多冗余信息。

七、开源的视觉vSLAM方案

1. 主流开源SLAM方案

2. 开源代码repo

2.1 gmapping

ROS封装的gmapping节点：slam_gmapping

gmapping的实现源码：openslam_gmapping

2.2 Hector

hector_sla

2.3 karto

ROS封装的karto节点：slam_karto

karto内部实现：open_karto

navigation_2d

2.4 cartographer

算法实现：cartographer

cartographer_ros

3. PTAM算法

3.1 PTAM算法简介

PTAM（Parallel Tracking ans Mapping）是基于关键帧的视觉vSLAM算法，该算法由牛津大学的 Georg Klein 和 David Murray于2007年提出。

PTAM是第一次将跟踪（Tracking）和建图（Mapping）分成两个独立线程的SLAM算法。其中，跟踪线程负责跟踪相机的位姿，同时绘制虚拟的模型；地图构建线程负责建立场景的模型和绘制场景的地图。目前，PTAM几乎成为了 feature-based SLAM 的标准方法。

3.2 PTAM算法的流程及框架

PTAM系统本质上是 MonoSLAM++ 系统，但因为采用了一种重量级算法（束调整）而显著提高了跟踪效果，并做到了实时。PTAM表明了实时束调整执行的方式。

4. LSD-SLAM

2014年ECCV提出LSD-SLAM（Large-Scale Direct Monocular SLAM），特点是：不适用边角或任何本地特征。

通过使用一种带有稳健的 Huber 损失的由粗到细的算法，直接跟踪可由图像到图像对准完成。这和那些基于特征的系统非常不同。深度估计使用了逆深度参数化（和许多其它系统一样）并使用了大量或相对小的基准图像对。该算法并不依赖于图像特征，而是靠有效地执行纹理跟踪。全局映射是通过创建和解决姿态图形的束调整优化问题而执行的，而且这所有都是实时工作的。这个方法是半密集的，因为它仅估计靠近图像边界的像素深度。LSD-SLAM 输出比传统的特征更密集，但并不如 Kinect 类型的 RGBD SLAM 那样完全密集。

工作中的 LSD-SLAM 同时生成一个摄像头轨迹和一个半密集的 3D 场景重建。这种方法实时工作，不使用特征点作为图元，并执行直接的图像到图像对准。

LSD-SLAM的变种算法有：

• 全方位 LSD-SLAM是 LSD-SLAM 的一种延伸，因观察到针孔模型不能用于大视场（理想情况下可超过 180 度）的观测而被创造出来，该成果提出于 IROS 2015；
• 立体 LSD-SLAM是 LSD-SLAM 的一种用于双眼摄像头套件的延伸。

5. Tango 项目

谷歌的 Tango 项目是世界上第一个商业化 SLAM 的尝试。Tango 项目展示了一种通过在图像到图像匹配矩阵中寻找特定的模式以进行环路闭合的新方法。这 个方法是来自没有固定位置的位置识别成果。

6. ElasticFusion

ElasticFusion 是一种需要 Kinect 这样的 RGBD 传感器的密集型 SLAM 技术，2-3 分钟就能获得单个房间的高质量 3D 扫描。许多 SLAM 系统的场景背后都使用了姿态图形，一种以地图为中心的方法。该方法专注于构建地图，但其诀窍是其构建的地图可以变形，也因此得名 ElasticFusion（弹性融合）。其中，算法融合部分是 KinectFusion，它是第一个高质量的基于 Kinect 的重建方式。