vslam流程框架介绍

平常扫地机产品上经常听说使用了vslam技术，那么这个vslam到底是什么呢，我们下面一起来看看。

vslam是什么？

VSLAM 即 Visual Simultaneous Localization and Mapping，主要是指如何用相机解决定位和建图问题。当用相机作为传感器时，我们要做的，就是根据一张张连续运动的图像(它们形成一段视频)，从中推断相机的运动，以及周围环境的情况。

通过上图我们可以清晰的看到，右上角是摄像头采集的一段行走过程的图像数据，画面上很多黄色标注的小方框，而图像的左边是一个运行的三角形，通过和右上角的图像结合，左边绿色三角形组成的轨迹似乎和右上角的记录轨迹一致的。这个就是vslam系统，清晰的可以看到左侧的定位与建图。

简单的了解了vslam的作用，那么它的框架又是怎样的呢？

VSLAM 的技术框架主要由5部分组成，包括传感器数据预处理、前端、后端、回环检测、建图。

上图清晰的标识数据流的处理流程，从传感器捕获图像数据后，传给前端，接着是后端，最后建图，而这个过程还有回环检测。具体每个模块都是实现了什么呢？下面逐个介绍，文中的内容，都是参考高翔博士的书籍进行学习梳理，如果小伙伴有需要更系统、完善的入门学习课程，推荐高翔博士主讲的深蓝学院视觉SLAM理论与实践的专门课程：深蓝学院视觉SLAM理论与实践。

VSLAM技术框架

图像数据由sensor传感器捕获，捕获图像之后，不会直接送到前端处理，而是有个预处理，预处理进行什么操作呢？

传感器数据预处理

传感器数据预处理，VSLAM中的传感器包括摄像头、惯性测量单元IMU等，预处理主要包含对图像进行滤波操作（中值滤波、高斯滤波等）、去畸变。

首先的，摄像头选怎样的呢？看一下下面的这个图。

• 单目：单个sensor传感器，BOM成本低，拍摄距离不受限，但是有个尺度不确定性问题，无法知道拍摄物体与相机的距离，同时，vslam初始化时，第一次求得帧间变化时，不能只有纯旋转，必须有一定程度的平移。
• 双目：两个sensor传感器，可以计算拍摄物体距离相机的位置（三角定位），也是拍摄距离不受限，但是双目计算深度信息，计算量比较大，也需要知道两个sensor传感器的距离。
• RGB-D：不但可以获取图像数据，而且主动测量每个像素的深度，对3D点云重建效果好，但是主动测量的范围比较小，而且测量容易受到太阳光干扰，同时对玻璃等透明材料无法测量。

在拿到图像数据之后，还会对图像进行滤波操作，一般是中值滤波或高斯滤波，滤波作用主要是去除图像中的噪声，减少干扰。滤波前后对比效果如下：

滤波之后，还会有去畸变处理。我们都知道，相机的镜片是凸透镜，由于镜片的折射、损耗等，导致真实环境的直线在图像中变成了曲线，畸变主要有枕型畸变和桶形畸变，如下图：

所以，为了后续模块可以更准确的得到图像信息，还需要对图像进行去畸变处理。

相机图像数据在经过预处理后，将来到前端模块，那么，前端又是做什么的呢？

前端(视觉里程计)

前端，又称为视觉里程计（visual odometry，简称VO），主要是研究如何根据相邻帧图像定量估算帧间相机的运动。通过把相邻帧的运动轨迹串起来，就构成相机载体（如机器人）的运动轨迹，解决定位的问题，然后根据估算的每个时刻相机的位置，计算出各像素的空间点的位置，就得到地图。

在介绍前端的具体操作之前，我们先来看看前端处理后的图像数据是怎样的。

上图是vslam开源算法 — ORB-VSLAM2 处理后的图像数据。图中左侧窗口中的绿色小方块为提取的图像ORB特征点，右侧窗口中的绿线代表相机的运动轨迹，蓝色方框代表相机运动过程中的空间位置（即关键帧），黑点和红点代表环境的稀疏地图（黑色代表历史路标，红色表示当前路标）。

上面说到的特征点，相邻帧估算帧间运动等，都是什么东西呢？

首先，依靠相邻帧估算帧间运动，得在图像中找个参照物吧，我们可以简单理解，这个参照物就是特征点，所以，估计估算帧间运动先对每一帧图像提取特征点。

特征点是怎么判定的呢，如何认为一个点是特征点呢？

FAST是一种角点，FAST算法定义特征点是如果某个像素点和他周围领域足够多的像素点处于不同区域,那么这个像素点就是特征点。

该算法的详细计算步骤如下：

1. 在图像中选取像素p；
2. 选取一个以选取点坐标为圆心的半径等于3的圆，该圆上共有16个点；
3. 判定该圆上是否有连续N个连续的像素点，这N个像素点的亮度值与圆心p的像素值的差大于或小于某一阈值；
4. 如果大于或小于某一阈值，则圆心p是特征点；

有时候在实际计算中，为了更高效，不一定都检测16个点，而是直接检测圆上的第1，5，9，13个像素的亮度，只有4个像素点中有3个满足差值，则认定为该点是特征点。

检测得到特征点之后，将还会获取特征点描述子，它是描述特征点与附件两个随机像素的大小关系，因为特征点很多，并不知道帧间特征点哪些是同一个，这个时候就需要描述子来表达，某一特征点周围都有什么，如何区分它们。

获取得到特征点之后，将会进行图像间的特征匹配，下面以一组图像来展示这个过程。

上面是两张相似的图像，右边的图像内容相对左边的向左移动了一些。首先的，针对两帧图像提取特征点，如果对提取出来的特征点直接匹配，将会变成下面的样子。

从上面可以看到，图中有很多错误的匹配，这些都是会影响最后的计算的，所以我们需要筛掉这些错误的匹配。怎么筛选呢，实际上就是计算特征点描述子的汉明距离，将差异比较大的特征点去掉，去掉之后变成下面这样。

从上图可以看到，当前剩余的匹配点基本是正确的了，那么得到相邻帧匹配的特征点之后，接下来要估算帧间的相机运动，将计算两帧图像间的旋转向量R和位移矢量t，单目相机从对极几何得到帧间关系(R、t)。

以上图为例，O₁，O₂为前后帧的相机中心，P为三维空间某一点，在O₁处拍摄得到的图像I₁中有一个特征点P₁，在O₂处拍摄的图像I₂有对应的特征点P₂，如果匹配正确，说明他们是同一空间点在两个成像平面上的投影。O₁，O₂，P三点确定一个平面，称为极平面，e₁，e₂为极点，O₁O₂为基线。

那么怎么计算这个帧间关系呢，下面从代数角度分析对极几何约束。

通过上面我们了解了是如何从两帧图像中求得旋转矩阵和位移矢量，但是2D-2D相机位姿估计存在几个问题：

1. 尺度不确定性：估计的位移矢量t是没有单位的，相机移动距离只有相对值，没有绝对值。所以将会使用第一帧和第二帧求得的位移矢量作归一化处理，后面的移动都是相对该结果的变化。
2. 初始化的纯旋转问题：单目初始化必须有一定程度的平移，否则 t 趋近0，导致 R 误差大；
3. 多于8对点的情况：使用特征点进行估计是采用随机采样一致性（RANSAC），有效避免错误特征点；

通过上面，我们就可以得到帧间相对变化，将其连串起来，就是一个轨迹地图了，单纯的只有前端处理，会是怎样的呢，为什么会需要后端呢，下面再看看后端处理。

后端

视觉里程计只计算相邻帧的运动，进行局部估计，这会不可避免的出现累积漂移，这是因为每次估计两个图像间的运动时都有一定的误差，经过相邻帧多次传递，前面的误差会逐渐累积，轨迹漂移（drift）的越来越厉害，这个过程，下图很好的展示了偏移情况。

上面的轨迹漂移问题，可以通过后端优化和回环检测这两个模块进行优化，避免误差累积越来越大。

后端，主要是对前端的结果进行优化，得到最优的位姿估计。也就是在什么样的状态下，最可能产生现在观测到的数据。那么具体是做什么呢，下图很好的解释。

在求解帧间的旋转矩阵和位移矢量过程，就像上图，知道一系列的(X, Y)，求解最合适的函数参数，使函数曲线到各个点的误差最小。

在机器人中，X和Y为观测方程（X：当前机器人的位姿(位置与姿态)，Y：观测到的路标点坐标（图像中的特征点)），实际上前端已经推导出观测方程，但是得到的观测方程和实际的观测值存在一定的误差，后端就需要将该误差最小化。如何使误差最小呢？

那就是观测值到观测方程中的距离最小，常用的是最小二乘法原则：以“残差平方和最小”确定方程。下面以经典的最小二乘法方案-----高斯牛顿法为例作为后端优化的介绍。

高斯牛顿法，基本思想是使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。

上面的其中 β \beta β 可以理解为vslam求解得到的R和t，而 J 则是方差对 β \beta β 的求导，当 Δ β \Delta\beta Δβ 足够小时，则停止优化；否则将代入下一个位姿点继续优化。

后端优化，实际工程上更多的会基于G2O这个图优化库进行最小二乘优化。（因为SLAM领域本身误差项比较多而且不知道之间的关联，同时本身这个系统也是非线性非高斯系统，所以使用图优化被大多数开发者采纳）。图优化将会优化问题表现为图，用顶点表示优化变量，用边表示优化项，将非线性最小二乘问题构建一个与之对应的一个图进行优化。本身G2O中就可以选择使用高斯牛顿法、列文伯格-马夸尔特方法等进行求解非线性最小二乘问题。

回环检测，主要目的是让机器人能够认识自己曾经去过的地方，从而解决位置随时间漂移的问题。

回环检测依据两幅图像的相似性确定回环检测关系，当前工程端采用词袋模型解决该问题。词袋也就是Bag-of-Words(BoW)，目的是用“图像上有哪几种特征”来描述一副图像。词袋存放着单词(Word)，多个单词组成字典，字典中单词的存放按照 k 叉树结构来存放。

为了可以正常的回环检测，将需要进行下面的操作：

1. 创建字典，将各帧图像的特征描述子存放到 k 叉树中；
2. 在创建字典的时候，将会计算各描述子的权重；
3. 对比图像进行相似度计算，确认图像中的特征点都对应哪些单词，计算帧间差异；

回环检测在字典应用上还需要注意下面的问题：

1. 增加字典规模，否则不同图像相似度差异会比较小；
2. 增加关键帧的处理，因为相邻的帧相似性比较高，回环检测的帧应该稀疏些；

建图

建图，将机器人移动的过程周围环境拼接起来，得到完整的地图，地图的用处主要有：定位、导航、避障、重建、交互等。

通过上图，我们可以清晰的了解到，每种地图可以用于什么场景。稀疏标记地图只能用于定位，而稠密地图还可以用于导航、避障、重建，语义地图则是用于交互，如VR等。

那么扫地机机器人，最起码需要是半稀疏半稠密地图，要不然无法实现导航，进行路径规划。单目相机又是如何实现重建地图的呢？

单目相机重建地图，需要知道每一个像素的距离，在多帧图像中，得到相机运动后，三角化计算像素的距离。需要用到极线搜索和块匹配技术找到帧间图像同一点对应的像素点（不像特征点，可以直接知道帧间特征点的对应关系，在这个需要对每个像素点都进行操作的，所以只能极线搜索）。

如上图，知道了p₁点，在经过运动之后，极线上的哪一点才是我们刚才看到的p₁点呢？

如果是特征点，可以通过特征点匹配找到p₂的位置，但是当前没有描述子，只能从极线上搜索p₂点。

可以从第二幅图像中的极线某一头走到另一头，逐个比较每个像素和p₁的相似程度，但是该方法可能有很多类似的点。在建图中，通过块匹配的方式来查找相似点。通过分块进行相似匹配，取两块的差的平方和或者归一化互相关计算相似性，得到帧间相似的像素，然后再三角测量得到该像素点的深度。

在得到帧间的旋转矩阵和位移矢量以及各像素点的位深之后，将可以构建PointCloud，接着就可以通过 PCL 点云库拼接为3D点云图。

总结

上面我们简单介绍了vslam的框架，很多地方只是简单的描述（PS：因为我也是一知半解），有些地方表达也存在一些问题。而像视觉里程计中会有使用光流法的，也没有进行介绍，像相机针孔成像模式、世界坐标与图像坐标的转换、李代数等都没有介绍，这些需要另外的去学习，了解，vslam涉及内容较多，后续将会继续学习，了解。

参考

本文主要参考了高博的《视觉SLAM十四讲》以及网上大量的优秀文章，感谢。