改进特征匹配的ORB-SLAM 稠密建图算法

刘洋，陈俊，胡诗佳，赖佳华

（1.福州大学 先进制造学院，福建 晋江 362200；2.中国科学院海西研究院泉州装备制造研究中心，福建 晋江 362200）

0 概述

同步定位与建图（Simultaneous Localization and Mapping，SLAM）指在未知环境下，机器人在运动中估计自身轨迹并构建周围环境的模型。使用相机作为传感器来解决定位与建图问题的一般称为视觉SLAM。MUR-ARTAL等［1］编写1 套基于特征点法的视觉SLAM 系统。文献［2］开源了支持单目、双目、RGB-D 3 种接口的ORB-SLAM2。文献［3］推出与混合地图数据关联的ORB-SLAM3，提出基于特征的紧耦合惯性视觉里程计，定位的精度和鲁棒性得到显著提升。然而，ORB-SLAM3 在跟踪阶段依旧使用暴力搜索描述子距离得到大量匹配点对，并结合比率测试筛选出置信度较高的匹配点对，或者将上一帧跟踪的地图点投影到当前帧，建立旋转直方图用于检测旋转一致性。这2 种方法用于特征点匹配，对于相机变换角度较大的场景容易产生误匹配，而且其建立的是稀疏地图。稀疏地图是指地图只建模特征点部分而不是所有看到过的部分，仅用于对相机进行定位，难以推断特征点之间的空间结构，从而无法实现导航、避障、重建等高层次的应用［4］。

针对特征匹配环节效率和准确度较低的问题，研究人员从不同的角度提出解决方案。从描述符入手，文献［5］提出一种基于稀疏光流与描述符无关的两阶段关键点匹配方法，文献［6］手工设计一种基于投票的环采样模式的特征组合二进制描述符，同时编码强度和梯度。文献［7-9］基于深度学习方法生成紧凑的二进制描述符，文献［10］比较了手工设计、经过训练的卷积神经网络（Convolutional Neural Network，CNN）、来自预训练的CNN 这3 类描述符，研究发现基于CNN 的描述符优于手工设计的描述符。研究人员引入其他特征来增强特征匹配的鲁棒性，文献［11-12］针对低纹理环境，增加线特征，结合点和线段2 种特征进行匹配。文献［13-15］采用更优的匹配策略以及外点剔除方法，提升匹配准确率。但以上方法会延长特征匹配时间。针对稀疏地图表达能力弱的问题，文献［16］将卷积神经网络与SLAM 相结合，生成带有语义标签的地图，文献［17］提出一种稠密映射框架来补充稀疏视觉SLAM 系统，将SLAM 系统生成的相机位姿、关键帧和稀疏点作为输入，通过TSDF 融合进行全局一致的稠密3D 重建。不同的地图类型适用不同的应用场景。

本文从提升特征匹配环节效率和生成一种易进行2 次加工的地图这2 个角度出发，在特征点匹配阶段提出一种改进的ORB-GMS 匹配策略并在建图方面加入稠密点云构建线程，从而提升定位精度与建图的效果。点云地图提供一个可视化功能，通过点云地图，构建满足导航需求的占据网格地图［18-19］，也可以进行泊松重建［20］满足三维重建的目的，从而扩大算法的使用场景。

1 相关原理

1.1 特征点法

经典视觉SLAM 框架由传感器信息读取、视觉里程计（Visual Odometry，VO）、优化、回环检测和建图5 个模块组成。其中，VO 的主要任务是通过相邻两帧图像确定相机的运动，计算相机位姿。根据实现原理的不同，VO 主要分为特征点法和直接法2类。特征点法具有较优的稳定性且对光照和动态物体不敏感，成为较主流的方式。特征点法的主要思想是对输入的图像进行特征点检测并提取描述子，然后进行特征匹配，计算相机位姿和建图。特征点指图像中一些辨识度高的像素点，一般图像中的角点、边缘和区块都可以当成图像中具有代表性的地方，其中在不同图像之间角点相比边缘或者区块的辨识度更强。常用的特征点法有FAST［21］、SIFT［22］、SURF［23］、ORB［24］等，ORB 特征点法能实现速度和质量的平衡［24］。ORB 特征由关键点和描述子2 部分组成。关键点称为Oriented FAST，是一种加入尺度不变性和旋转不变性的FAST 角点［21］，描述子称为BRIEF［25］，用于在不同图像中匹配的特征点。由于BRIEF 描述子是二进制的，因此比较2 点的特征非常快，通过暴力匹配另一帧上的所有特征点，计算最小的汉明距离得到最佳匹配。但是该方式在重复纹理或者缺乏纹理信息的区域内会产生大量的误匹配，因此后续还会采用一些方法剔除误匹配。

1.2 ORB-SLAM3 算法框架及原理

2015 年西班牙萨拉戈萨大学开源ORB-SLAM。2021 年ORB-SLAM3 被提出。基于ORB-SLAM 和ORB-SLAM2 的代码框架，ORB-SLAM3 依旧采用3 个线程，分别是跟踪、局部建图和闭环&地图融合。ORB-SLAM3 在之前工作的基础上实现了基于视觉与IMU 的紧耦合SLAM 系统，完全依赖最大似然估计，使得稳定性大幅提升，并且与以前的方法相比精度提高了2～10倍［3］。

图1 所示为OpenCV 和ORB-SLAM3 在同一幅图像上提取ORB 特征点的结果（彩色效果见《计算机工程》官网HTML 版）。ORB-SLAM3 采用的ORB特征点是一种改进的FAST 角点，并添加了尺度和旋转描述。通过比较中心像素与周围像素的亮度来判断该点是否为特征点。由此产生的角点经常出现“扎堆现象”，OpenCV 提取的特征点如图1（a）所示。ORB-SLAM3 采用多种策略提升特征点质量，利用非极大值抑制和四叉树均匀分布策略解决角点集中的问题，通过高斯模糊原图像提取更显著的特征点，构建图像金字塔以实现尺度不变性，通过灰度质心法增加旋转不变性。ORB-SLAM3 在提取关键点之后，还需计算相对应的二进制描述子。从图1 可以看出，ORB-SLAM3 提取的特征点数更多且更均匀。

图1 OpenCV 与ORB-SLAM3 提取ORB 特征点对比Fig.1 Comparison between OpenCV and ORB-SLAM3 in extracting ORB feature points

两帧间的特征匹配是为了确定不同帧之间特征点的对应关系。通过准确匹配图像与图像或者图像与地图之间的描述子，为后续的姿态估计和后端优化降低计算量。暴力匹配是ORB-SLAM3 最常用的特征匹配方法，通过计算当前图像上每个特征点与另一幅图像上所有特征点的BRIEF 描述子的汉明距离，并对其进行排序，取最近的1 个特征点作为匹配点。ORB-SLAM3 先采用ratio test（若最近的特征点距离除以次近距离小于某个阈值，则接收这对特征点）筛选出一部分比较显著的匹配特征点，然后在特征点的一定半径内快速搜索候选匹配特征点，计算旋转角度，并放入将360°分为30 个小格子的直方图中，利用角度一致性检测过滤掉角度差超过12°的外点，从而快速得到相对“正确”的匹配特征点对。然而，对于角度差在12°以内的错误匹配或者是相机视角变化较大的场景，ORB-SLAM3 就无法得到正确的匹配。

2 改进方案

2.1 改进的ORB-GMS 方法

基于网格的运动统计（Grid-based Motion Statistics，GMS）算法［26］是一种基于网格运动统计的快速鲁棒的特征匹配方法，主要用于多视图立体（Multi-View Stereo，MVS）三维重建领域，以解决因相机角度变化较大而存在相邻两帧对应特征不显著的问题。本文对该方法进行改进，融合ORB-SLAM3的特征匹配阶段，在提升特征匹配准确率的同时缩短所用时间。本文基本思想是正确匹配满足于运动平滑性约束，即1 个正确匹配点对的旁边肯定还会有几个正确的匹配点对来支撑，这些匹配点具有运动一致性，而1 个错误匹配点对（假设错误是随机发生的）旁边的支撑匹配点对几乎很少，其原因为很难找到出现同样错误的匹配点对。因此，通过统计特征点邻域内的匹配点总数来判断当前匹配是否正确。针对SLAM 算法实时性要求高和相机拍摄帧率较快的特点，本文放弃使用GMS 原本扩展的运动核，舍弃旋转不变性和尺度不变性，追求更高效的特征匹配。网格化图片可以加快算法效率，使得原本中心点邻域由圆变成了3×3 的网格，而GMS 忽视了不同网格到中心点的距离和置信度不同。因此，本文根据与中心网格的距离，对各网格赋予不同的权重，以达到更加公平计数的目的。

假设图像Image1 和Image2 分别有p、q个特征点，xi={pi，qi}表示第i个点邻域内的特征点匹配对，|xi|表示数量，定义匹配点邻域支持度Si=|xi|-1。为加快算法效率，将图像分成20×20 的网格，通过网格化来加速运算。把原本邻域内匹配点的问题简化为网格的对应匹配问题，选择匹配点对附近的3×3网格作为邻域，如图2 所示。

图2 特征点对的运动核Fig.2 Motion kernel of feature point pairs

本文定义网格的邻域为它周围的8 个网格，其中a1、a3、a7、a9与中心网格a5的距离是a2、a4、a6、a8到a5距离的倍，因此支持度Sab的计算式更新为：

其中：|xakbk|表示ak到bk匹配点对数量；Sab表示匹配点支持度的得分。

假设每个特征点匹配都是相互独立的，那么支持度的概率分布近似于二项分布：

其中：|xi|代表第i个点邻域内特征点的匹配数量；pt和pf分别表示正确匹配和错误匹配在其对应邻域内有支持度支持的概率。pt通常取决于特征质量，与匹配的正确率接近，而pf通常很小，误匹配的分布几乎是随机的，其对应的期望与方差如下：

正确匹配和错误匹配支持度对应的概率分布如图3 所示。

图3 正确匹配和错误匹配支持度对应的概率分布Fig.3 Probability distribution corresponding to support for correct matching and wrong matching support

由两者的分布可看出正确匹配点和错误匹配点具有可分性，并且根据GMS［26］的推导，可分性正比于，说明随着特征匹配点数量的增多，两者的期望相差会越来越大，可分性也越来越明显。阈值设置为：

阈值越大说明划分标准越严格，匹配点对更可能是正确的匹配，由GMS［26］得知，a在4～6范围 内时，结果较优。因此，本文取α=6。当支持度Si大于阈值时，说明pi和qi是1 对正确的匹配，否则是1 对错误的匹配。

2.2 稠密点云地图与滤波

ORB-SLAM3 只能生成稀疏点云地图，因此，本文在原算法框架上加入稠密点云构建线程，利用RGB-D 相机进行稠密重建。RGB-D 相机可以通过传感器中的硬件测量得到深度，无须消耗大量的计算资源来估计。在ORB-SLAM3 3 个线程的基础上，增加1 个稠密点云构建线程，首先匹配RGB 图像与深度图，然后根据前端估算相机位姿，在新增关键帧时将位姿信息和深度信息传入稠密点云构建线程，计算图像上所有像素的空间坐标，生成点云图。利用RGB 图像信息给点云上色，随着关键帧不断新增，与生成的地图不断进行点云拼接和全局优化，最后得到1 个由离散点组成的彩色点云地图，实现稀疏到稠密的转变。本文算法架构如图4所示，中间虚线方框部分为新增的稠密点云构建线程。

图4 本文算法架构Fig.4 Architecture of the proposed algorithm

经过遍历匹配好的RGB 图像和深度图，提取像素坐标和深度值，根据针孔相机成像模型可得：

其中：Z为深度值和实际空间距离的比例关系因子；K为相机内参矩阵；R和t为相机的旋转矩阵和平移矩阵；PW为像素点在世界坐标系下的三维坐标。由于相机已进行标定，因此内参矩阵K为已知量，通过跟踪线程来计算相机位姿，以确定R和t，进一步确定RGB 图像中每个像素的空间坐标。本文调用PCL（Point Cloud Library）库处理计算得到的点云数据。点云pi的坐标可由式（6）计算得出：

利用RGB 图像三通道的色彩信息给点云上色，然后读取下一个关键帧并进行点云拼接。

在实际构建稠密点云地图时存在如下问题：1）产生的点云非常冗余，由于多个视角存在视野重叠，因此在重叠区域内会存在大量位置十分相近的点；2）受RGB-D 相机天然技术缺陷的影响，导致一些部位的深度测量出现错误，如深黑色物体、表面光滑物体、透明物体，产生的点云杂乱。为获得更好的视觉效果，本文加入外点去除滤波器和体素网格的降采样滤波器，把平均距离在标准范围之外的点视为离群点并从数据中去除。假设所有的点云在1 个大立方体的空间内，根据体素滤波器中设定的分辨率0.01 m×0.01 m×0.01 m，将该大立方体分割成1 个1 cm3的立方体，求解每个小立方体的质心坐标，用该坐标近似表达整个小立方体，相当于对三维空间进行降采样，从而节省存储空间。

3 实验与结果分析

3.1 实验环境

本文选用TUM 中RGB-D 数据集［27］的Handheld SLAM 类别进行实验，该数据集主要是在室内场景下进行统计，纹理比较丰富且场景深度较小，有利于特征点法发挥优势。大部分图像因相机的移动而模糊，有利于验证本文改进方案的有效性。实验平台是搭载Ubuntu18.04 系统的笔记本电脑，处理器是Intel i7-9750H，主频2.60 GHz，运行内存16 GB。

3.2 特征匹配实验

特征匹配优劣的评价通常从速度和匹配数量2 个方面进行分析。因此，本文从每个数据集中随机抽取相近的两帧进行特征匹配，每帧提取1 000 个特征点，模拟实际运行时关键帧间的特征匹配。不同方法在特征匹配所花费的时间以及匹配的特征点数量如表1 所示，加粗表示最优数据。从表1 可以看出，本文算法缩短了特征匹配所需的时间，并且在特征点匹配数量上也有大幅度提升。

表1 不同方法的特征匹配实验结果Table 1 Experimental results of features matching among different methods

图5所示为ORB-SLAM3、ORB-GMS 和本文算法筛除误匹配的结果。

图5 ORB-SLAM3、ORB-GMS与本文算法筛除误匹配的结果Fig.5 The results of ORB-SLAM3，ORB-GMS and the propoesd algorithm for screening out mismatches

按平均表现来看，本文算法在匹配耗时上约为ORB-SLAM3 时长的1/2，匹配的特征点数比ORB-GMS 略高，相较于原ORB-SLAM3 匹配的特征点数提高了60%，说明本文算法克服ORB-SLAM3 效率低且误匹配率高的缺点。本文算法对于视角变化较大的两帧具有鲁棒性，以fr3/lo 为例，两帧的相机视角变化较大，在图5 的左图中相机位于办公椅的侧后方，无法看见办公桌左侧的垃圾桶，在图5 的右图中相机移到了办公椅的正后方，能看见垃圾桶。左右图的特征点差别较大，导致ORB-SLAM3 只有75 对匹配点，而本文算法是其3.5倍，说明在相机快速移动的情况下，本文算法具有更优的鲁棒性。

3.3 SLAM 算法精度实验

本文从时耗、复杂度、精度等多个角度评估SLAM 算法的表现。其中，对精度的评价主要使用绝对轨迹误差（Absolute Trajectory Error，ATE）和相对轨迹误差（Relative Pose Error，RPE）2 个精度指标。绝对轨迹误差是相机位姿的真实值与SLAM 系统估计值之间的差值，适合评估算法精度和相机轨迹的准确性。而相对轨迹误差计算在相同2 个时间戳上估计位姿变化量与真实位姿变化量的差，适合估计系统的漂移。本文算法针对特征匹配环节，旨在获得更加精确的位姿，并不会对系统的漂移产生影响，也通过实验得到验证，在此并不赘述。因此，本文选择绝对轨迹误差来判断定位精度是否提升，并且选用平均误差（MEAN）、均方根误差（RMSE）、误差平方和（SSE）这3 个指标来反映绝对轨迹误差的变化。

在各个数据集上ORB-SLAM3 和本文算法的绝对轨迹误差对比如表2所示。本文把RMSE 变化量小于0.01 m 或变化幅度在10%以内的视为无明显变化，用“—”表示，变化量大于0.01 m 且变化幅度小于-10%视为有下降趋势，用“↓”表示，变化量大于0.01 m 且变化幅度大于10%视为有上升趋势，用“↑”表示。

表2 ORB-SLAM3 与本文算法在TUM 数据集上的绝对轨迹误差对比Table 2 Comparison of absolute trajectory error between ORB-SLAM3 and the proposed algorithm on the TUM datasets 单位：m

在fr1/360、fr1/fl、fr2/de、fr2/wl 数据集上，本文算法的MEAN、RMSE、SSE 均得到有效降低，除fr2/he数据集以外，在其他大部分数据集上都保持与ORBSLAM3 一样的精度。经过观察分析数据集的视频序列，误差无变化的数据集相机移动缓慢且场景纹理比较丰富，这也意味着相邻两帧间的变化非常小，ORB-SLAM3 方法也能得到较正确的匹配，本文算法没有发挥其优势，但是可以减少匹配的耗时。在fr2/he 数据集上本文算法表现不佳的原因是相机在地面与天花板之间来回移动，而天花板是一大片重复纹理，反而增多了误匹配点的数量，从而导致误差增大，说明本文算法在处理重复纹理场景时存在局限性。但本文算法在发挥作用时，明显展现出它的优势，绝对轨迹误差的各个指标全面下降，平均降低约32%。

Fast ORB-SLAM［5］以及PL-SLAM［12］算法同样是通过提高匹配的准确度来提高定位精度，本文在TUM 数据集不同的序列下比较3 种算法的定位误差，将绝对轨迹误差的均方根误差（RMSE）作为评价指标，结果如表3 所示，表中数据来自文献［5，12］，“—”表示无法从文献中得到相应的值，加粗表示最优数据。本文算法总体具有更优的准确度。

表3 不同算法在TUM 数据集下的均方根误差对比Table 3 Root mean squared error comparison among different algorithms on TUM dataset 单位:m

3.4 稠密建图实验

稀疏地图以及滤波前后的稠密点云地图效果对比如图6 所示，图6（a）所示为ORB-SLAM3 的可视化运行界面，生成的稀疏地图对人眼不友好。由于稀疏地图中点与点之间没有结构，无法展现立体感，因此人们无法理解地图保存的路标点。通过稠密点云构建线程，利用RGB-D 相机获取的深度信息生成三维彩色空间点云，利用统计滤波器去除外点并采用体素滤波对生成的点云进行降采样。表4 所示为在fr1/fl 数据集上，对比滤波前后的点云数量、生成模型大小。从表4 可以看出，滤波后冗余和杂乱的点云数量急剧减少，存储空间也大幅缩减，均只有滤波前的1/4，而建图效果下降不明显，有利于大场景下的稠密建图。

表4 滤波前后点云数量与模型大小对比Table 4 Comparison of point cloud number and model size without filtering and after filtering

图6 稀疏地图以及滤波前后的稠密点云地图效果Fig.6 Sparse map and dense point cloud map without filtering and after filtering

4 结束语

针对ORB-SLAM3 中特征匹配环节费时且不准确的问题，本文提出一种改进的ORB-GMS 匹配策略，将运动平滑性约束作为特征点运动统计的方法，把图像网格化，对不同的网格按距离赋予不同的权重，通过比较与特征点相邻的8 个网格内匹配点对数量和预设阈值的大小来快速判断当前匹配是否正确。针对ORB-SLAM3 未能生成稠密地图的问题，本文基于PCL 库添加1 个点云构建线程，实现稠密建图并进行滤波处理，得到更符合人们可视化习惯且能进一步加工处理的稠密点云地图。实验结果表明，本文算法能有效提高特征匹配的效率和定位精度，为后续更高层次的应用奠定基础。下一步将研究如何利用图像的全局信息来解决特征点法在重复纹理或无纹理场景下产生的退化问题。