基于改进ORB-SLAM2 算法的RGB-D 稠密地图构建

韩彦峰 ，唐超超，肖科

（重庆大学 机械与运载工程学院，重庆 400044）

近年来，自动驾驶技术、VR 技术、无人机日新月异的发展，对地图和定位的精度需求愈来愈高.作为各种创新性技术中的关键环节，地图和定位精度的可靠性和准确性具有重要的意义，同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）成为目前国内外学者的热门研究领域［1-4］.SLAM 技术可搭载雷达、相机等传感器，因此基于相机传感器的视觉SLAM 技术相比于激光SLAM 技术具有设备元件成本低、功耗低等优点，更受到研究者们的青睐.相机可提供鲁棒性更优质的图像信息，能够更准确地识别位置信息，而位置识别是SLAM 技术系统中的关键模块.

2007 年，Klein 等人［5］提出的PTAM 系统，是视觉SLAM 发展中重要的里程碑，一举开创了多线程SLAM 系统的先河，将前端跟踪线程和后端优化建图线程并行化处理，后端优化开创性地采用了非线性优化方案，而非传统的滤波器优化（如卡尔曼滤波［6］、粒子滤波［7］）.2014 年，Endres［8］团队设计完成了基于深度相机的RGB-D SLAMV2 系统，首次利用RGB-D 相机特性进行稠密点云地图的构建.2015年，Mur-Artal 等人［9］提出了ORB-SLAM 系统，在PTAM 算法框架上增加了地图初始化和闭环检测环节，能够很好地消除运行产生的累积误差，显著提升了地图跟踪的快速性和构建地图的准确性，也成为了一个完整的SLAM 系统.2016 年，原团队在此基础上提出了ORB-SLAM2 系统［10］，在回环检测模块后又增加了全局BA（Bundle Adjustment）环节，提高了系统的鲁棒性；支持单目、双目、RGB-D 等多种常见相机.2018 年，Bescos 等人［11］提出基于ORB-SLAM2的DynaSLAM 算法系统，采用Mask-CNN 算法检测潜在动态物体，增加了动态目标检测和背景修复的能力，在单目、立体和RGB-D 配置的动态场景中非常强大；同年，清华大学提出了DS-SLAM 算法［12］，采用SegNet 进行语义分割并建立语义八叉树地图，精确定位图像场景下的各个物体.2021年，中国科学技术大学团队提出一种新的基于稀疏特征的视觉SLAM算法（DP-SLAM［13］），该算法基于移动概率传播模型进行动态关键点检测，结合几何约束和语义分割的结果，在贝叶斯概率估计框架中跟踪动态关键点，克服了几何约束和语义信息的偏差，提高了视觉SLAM系统的准确性和鲁棒性.

ORB-SLAM2 系统具有精确度高、实时稳定性好、应用场景多等优点，但存在如下缺点：所构建的地图比较稀疏而非稠密，点云地图存储空间大，无法有效描述空间占据状态导致无法满足导航、路径规划以及高层次的人机交互［14-17］.针对上述问题，本文提出一种基于RGB-D 模式下ORB-SLAM2 框架的VSLAM 方案，添加稠密建图线程，实时记录关键帧位姿进行点云拼接，引入双层滤波算法降低点云冗余度，位姿稠密回环处理优化点云空间位置，并最终转换成八叉树地图.本文测试多个开源数据集，在轨迹精度和建图效果等方面均有提高，验证了改进算法的有效性.

1 ORB-SLAM2系统框架简介

ORB-SLAM2 是一种前端视觉里程计基于“Ori⁃ented FAST”关键点和BRIEF 描述子，实现特征点的提取匹配，后端基于非线性优化BA 方式的视觉SLAM 系统.系统共有三个线程，由跟踪线程（Track⁃ing）、局部建图线程（Local Mapping）和闭环检测线程（Loop Closing）组成［18］.RGB-D 模式下，跟踪线程负责根据提供的特征点深度信息，进行实时位姿的定位跟踪并优化处理.局部建图线程通过所获得的关键帧创建新地图点并剔除地图外点，将关键帧的位姿进行局部BA 优化，删除冗余关键帧和地图点.闭环检测线程利用数学模型评价相邻关键帧的相似性程度，判断关键帧闭环情况，有助于减少轨迹的累积漂移.

2 改进RGB-D ORB-SLAM2系统框架

本文VSLAM 系统方案在原有RGB-D 模式下的ORB-SLAM2算法框架上进行扩展，添加稠密建图功能，使其能够对周围场景实时构建点云地图，并转换为可表征周围环境的3D 八叉树地图.系统整体构架如图1 所示，图中虚线方框内表示本文主要的扩展工作.

图1 改进RGB-D ORB-SLAM2系统架构Fig.1 Improved RGB-D ORB-SLAM2 system architecture

2.1 稠密点云地图与八叉树地图构建

2.1.1 稠密点云地图构建原理

RGB-D 相机提供的深度图像和彩色图像实现二维坐标转换为三维点云数据.定义任意一个空间三维点X，坐标表示为X=[xw，yw，zw]，在相机坐标系下三维点所对应已知点的像素坐标表示为：x=[u，v，1]，根据相机针孔模型成像原理可以变换得到点云的空间位置信息：

式中：K代表相机内参，由相机标定所得；R代表旋转矩阵，t代表平移向量；z是深度值和实际空间距离的比例关系因子.

世界坐标系原点与相机坐标系原点重合，即没有旋转和平移，所以R为单位矩阵，t为零矩阵，通过公式（1）变换得到点云的三维空间位置坐标：

2.1.2 八叉树地图的构建与更新

本文提出了一种在导航中比较常用的、压缩性能好的构建八叉树地图方式.如图2（a）所示，一个大立方体不断均匀地分成八块，直到变成最小的方块为止，白色表示未被占据，黑色表示被占据不可再分割.图2（b）中为八叉树结构，每一个节点类比成体素，未被占据的节点继续展开形成八个子节点，被信息占据的节点停止展开.当向地图中添加信息时，由于实际的物体经常连在一起，空白的地方也会常常连在一起，所以大多数八叉树节点无须展开到叶子层面［19］，因此八叉树地图比点云地图节省了大量存储空间.

图2 八叉树示意图Fig.2 Diagram of the octree

八叉树节点存储了它是否被占据的信息，从点云层面来讲，可以用0 表示空白，1 表示被占据.由于噪声的影响，会造成节点所存储的占据信息判别不准确，出现未占据、占据两种情况往复循环的现象，甚至存在未知状态.因此本文提出使用概率对数值（Log-odds）来表达某节点是否被占据.设y∈R为概率对数值，x为0～1 的概率，那么它们之间的变换由公式（3）给出logit变换描述：

某节点不断观测到占据时，y值增加；观测到空白时，y值减小.用数学形式来表示，设某节点为n，观测数据为z，那么从开始时刻到t时刻某节点的概率对数值为L(n|z1：t)，t+1时刻为：

通过公式（4）可以计算节点某时刻对数概率值，εoccupy为预先设定的参数，用于计算节点某时刻的概率对数值：

通过所得的对数概率值，结合RGB-D 数据就可以更新出整个八叉树地图.在RGB-D 图像中观测到某个像素带有深度d，就说明在深度值对应的空间上观察到了一个占据信息，并且从相机光心出发到这个点的线段上应该是没有物体的，否则会被遮挡［20-24］.利用此信息就可以很好地对八叉树地图进行更新，并且能处理运动的结构.

2.2 双层滤波处理

实际建图过程中，RGB-D 相机的深度量程有限，则需要根据实际相机的量程来设定上下限，滤除掉采集不到的点云.为了建图效果达到更优，本文对所采集到的点云进行体素网格降采样滤波器和外点去除滤波器双层滤波处理，使得稠密地图更加清晰、准确，带来更好的视觉效果.

2.2.1 离群点滤波

由于在点云拼接过程中，常常会产生稀疏的离群点，导致稠密点云配准失败，出现稠密点云地图效果不好的问题.为了解决此问题，使用外点去除滤波器对每个点云的邻域进行一次统计学分析，设置一定阈值修剪掉那些不符合一定标准的点.

本文通过对输入数据中点到其邻近点的距离分布计算，得到每个点到其所有邻近点的平均距离.由于结果近似为高斯分布，均值μ和标准差σ决定其形状分布，平均距离在标准范围（由全局距离平均值和方差定义）之外的点，可被定义为离群点，并可从数据集中去除掉.根据高斯分布d～N(μ，σ)模型化距离参数，计算所有点与其邻近点的均值和标准差，公式如下：

式中：dij表示每个点到其邻近点的距离，i=[1，…，m]表示共有m个数据点，j=[1，…，k]表示每个点都有k个相邻点，表示其与邻近点的距离均值.

2.2.2 降采样体素滤波

邻近关键帧相互存在视野重叠，重叠区域会存在大量位置相近的点，占用大量内存空间.本文对余下点云使用体素网格滤波器进行降采样，保证在某个一定大小的立方体（体素）内只保留位于立方体重心上的点［20］.这样既可以保持点云的形状特征，又能极大地降低全局点云的冗余，减少了点云存储所占用的内存空间.

根据坐标集合，求取X、Y、Z三个坐标轴上的最大值xmax、ymax、zmax和最小值xmin、ymin、zmin，然后根据三个坐标轴上的最大值和最小值求得点云最小包围盒的边长lx、ly、lz：

设置体素小栅格边长为lcell，将三个坐标轴分别均等化分为M、N、L份，最终将整个空间划分成Sum个体素小栅格：

对所划分的每个体素小栅格进行标号处理，标号为(i，j，k)，确定每个点云所属的小栅格坐标位置：

计算每个体素小栅格重心坐标，以重心代替该小栅格内的所有点云数据；当重心点坐标不存在栅格内时，取小栅格内在距离上最接近重心点的点坐标表示该小栅格内的所有数据点云的坐标，至此完成整个精简过程：

式中：cijk、pi、k分别为体素小栅格的重心、数据点、点数.

2.3 稠密点云回环处理

ORB-SLAM2 系统回环融合时，将执行全局BA优化.如图3（a）所示，通过图优化模型［21-23］，将关键帧位姿和地图点设置成图顶点为待优化变量，使用特征点法获取两个位姿节点之间相对运动的估计作为图边，以此省却大量特征点优化计算，仅把它们当作位姿估计节点的约束，从而构建成位姿图（Pose Graph），如图3（b）所示.保留了关键帧位姿并优化，并将系统中的位移累积误差均摊到每一个关键帧上，以此校正整个轨迹，使其更加准确.

图3 位姿优化原理Fig.3 Principle of position optimization

本文在此基础上提出一种稠密点云回环处理的方法，具体流程如图4 所示.系统基于词袋模型进行回环检测优化时，记录每一个关键帧位姿数据，系统判定为回环状态后，执行全局BA 优化更新关键帧的位姿与地图点坐标，将优化后每个关键帧位姿的点云重新进行拼接并进行滤波处理，更新优化局部点云地图，极大地提高了系统的鲁棒性，减小了误差，保证稠密点云地图的全局一致性.

图4 稠密点云回环流程图Fig.4 Dense point cloud loopback flow chart

3 试验结果和分析

3.1 稠密建图效果评估

为了验证本文改进算法的性能效果，与参考文献［24］同样基于ORB-SLAM2 系统提出的建图算法（GX ORB-SLAM2）进行试验比较评估.本文选取了部分慕尼黑工业大学发布的开源TUM 数据集［25］，详细信息见表1.

表1 TUM数据集信息Tab.1 TUM dataset information

搭建测试环境配备CPU：Intel Core i7-9700 @3.00GHZ、GPU：NVIDIA GTX1650、24G 内存、512G 固态和Ubuntu18.04操作系统.实验中使用G2O、Pango⁃lin、Eigen、OpenCV 等第三方软件库进行算法优化与可视化展示，本文设置滤波参数深度范围为0.01

本文RGB-D ORB-SLAM2 系统与GX ORBSLAM2 系统都可以实时构建场景的稠密地图，对其所挑选的4 个数据集进行试验对比，两者建图效果如图5和图6所示.

图5 fr2_xyz数据集下的稠密点云地图Fig.5 Dense point cloud map under fr2_xyz dataset

图6 fr1_desk2数据集下的稠密点云地图Fig.6 Dense point cloud map under fr1_desk2 dataset

从图5 和图6 可以看出：本文RGB-D ORBSLAM2 系统与GX ORB-SLAM2 系统在“fr2_xyz”和“fr1_desk2”数据集下都能实现三维场景稠密构建的目标，稠密点云地图并未出现明显位置错位且还原度较高，满足场景构建的基本需求.如图5（b）和图6（b）红色椭圆部分，可以看出GX ORB-SLAM2 系统得到的未滤波稠密点云地图存在大量的空间无用杂点，占用空间内存；本文系统经过双层滤波处理后其稠密地图滤去了超过精确测量范围外的点云，基本不存在冗余杂点.两个SLAM 系统在四个数据集下的点云数量进行对比，本文RGB-D ORB-SLAM2 系统构建的稠密点云地图的点云数均少于GX ORBSLAM2系统，点云数量大幅度降低，详细对比数据如表2所示.

表2 稠密点云数量对比Tab.2 Comparison of the number of dense point clouds

下面对数据集的稠密点云地图中的某些细节部分进一步详细比较，看是否存在局部错位缺失与模糊残影的情况，如图7所示.

两个系统在“fr1_desk2”数据集下的详细放大部分如图7（a）和图7（b）所示，重点观察图中红色椭圆部分.图7（a）中为GX ORB-SLAM2 系统所得，最左边的书本明显出现了局部模糊残影的情况，与中间的书本发生了轮廓模糊融合，纹路不清晰.图7（b）中为本文RGB-D ORB-SLAM2 系统所得，三本书的轮廓清晰，还原度高，左侧两本书未发生模糊融合，分界轮廓线明显.

图7 稠密点云地图细节部分Fig.7 Detail section of the dense point cloud map

综上可知，本文RGB-D ORB-SLAM2 系统在点云数量、冗余度、建图局部细节部分都明显优于GX ORB-SLAM2系统.

3.2 轨迹误差评估分析

TUM 数据集提供了相机位姿的真实轨迹，根据系统获得的估计轨迹，利用Evo 评估软件［26］可以分别对两者的绝对轨迹误差（ATE）进行分析，适用于评价系统的性能；相对位姿误差（RPE），适用于评估系统的漂移程度.

针对本文改进的RGB-D ORB-SLAM2 系统与RGB-D SLAMV2 系统，分别对所选取的四个数据集进行绝对轨迹误差（ATE）和相对位姿误差（APE）对比分析，具体分析结果如图8～图11所示.

图8 fr1_xyz数据集下的绝对轨迹误差Fig.8 Absolute trajectory error under fr1_xyz dataset

图8～图11 的横纵竖轴为三维空间下的X、Y、Z轴，灰色虚线reference 表示各个数据集真实轨迹值，彩色实线代表系统所获得的估计轨迹，图右侧从蓝到红的色带显示误差大小［19］.对比图8 和图9 可得，在数据集“fr1_xyz”和“fr2_xyz”场景下，本文RGB-D ORB-SLAM2 系统的红色轨迹都远少于RGB-D SLAMV2 系统，表明前者真实位姿与估计位姿的轨迹最大误差明显小于后者.因此，验证得到改进后的算法定位精度更准确，轨迹更具有全局一致性.

图9 fr2_xyz数据集下的绝对轨迹误差Fig.9 Absolute trajectory error under fr2_xyz dataset

图10 和图11 给出了基于数据集“fr1_desk2”和“fr3_long_office_household”的相对位姿误差分析，可以看出本文RGB-D ORB-SLAM2系统的轨迹重合度高于RGB-D SLAMV2 的轨迹重合度，绝大部分实线估计轨迹与虚线真实轨迹重合，无较大轨迹错位，说明本文改进系统的漂移稳定性更高.为了更加直观地评价系统性能的提升，表3和表4使用均方根误差（RMSE）和均值误差（Mean），表5 使用均方根误差和标准差（SD）作为主要评价指标，取两个指标数据5次平均值来定量分析系统的定位精度和系统漂移.

图10 fr1_desk2数据集下的相对位姿误差Fig.10 Relative positional errors in the fr1_desk2 dataset

图11 fr3_long_office_household 数据集下的相对位姿误差Fig.11 Relative positional errors in the fr3_long_office_household dataset

表3 RGB-D ORB-SLAM2与RGB-D SLAMV2绝对轨迹误差评价指标对比Tab.3 Comparison of RGB-D ORB-SLAM2 and RGB-D SLAMV2 absolute trajectory error evaluation metrics

表3 和表4 的RMSE1、RMSE2、Mean1、Mean2 代表RGB-D SLAMV2、RGB-D ORB-SLAM2 系统所得的均方根误差与中值误差；表5 和表6 的RMSE、SD代表DynaSLAM 所得.具体分析数据可知，静态场景下或低动态场景下，本文系统的绝对轨迹误差（ATE）与相对位姿误差（APE）的RMSE、Mean、SD 均小于RGB-D SLAMV2、DynaSLAM 计算所得，本文改进系统的两项评价指标至少都有50%～70%的提升，特别是在低动态帧序列数据集“fr1_xyz”与“fr3_long_household”下的绝对轨迹误差（ATE）评价指标对比上，本文系统提升幅度明显，而动态场景“fr3_walking_xyz”、“fr3_walking_halfsphere”与Dy⁃naSLAM 相比还有一定差距.验证了本文系统在低动态场景下具有更好的定位精度.

表4 RGB-D ORB-SLAM2与RGB-D SLAMV2相对位姿误差评价指标对比Tab.4 Comparison of RGB-D ORB-SLAM2 and RGB-D SLAMV2 relative positional error evaluation metrics

表5 RGB-D ORB-SLAM2与DynaSLAM 绝对轨迹误差评价指标对比Tab.5 Comparison of RGB-D ORB-SLAM2 and DynaSLAM absolute trajectory error evaluation metrics

表6 RGB-D ORB-SLAM2与DynaSLAM 相对位姿误差评价指标对比Tab.6 Comparison of RGB-D ORB-SLAM2 and DynaSLAM relative positional error evaluation metrics

4 构建八叉树地图测试

八叉树地图的可更新性和可优化性明显优于稠密点云地图，可满足更高精度需求的导航任务.本文对所选取的四个数据集进行点云地图转换构建到分辨率为0.05、具有颜色信息的三维八叉树地图［27］，具体建图效果如图12所示.

从图12 可以看出，对所选取的四个数据集构造的八叉树地图重要信息依然保留完整，如电脑、桌子等，可以清楚地分辨物体之间的位置关系，说明八叉树地图在节省大量存储空间的同时，完整地保留了地图的重要信息.表7 给出了在四个数据下八叉树地图与点云地图所占内存空间的大小.

图12 带颜色的八叉树地图Fig.12 Map of octree with colors

由表7 可知，在同一环境下，相比于稠密点云地图，八叉树地图的存储空间呈指数式下降，实现了由兆存储到字节存储的转变，极大地减少了内存消耗.八叉树地图用来存储描述地图的占据信息，可以更好地实时扩展、低消耗地满足未来移动机器人对高精度导航的需求.

表7 点云地图与八叉树地图存储空间Tab.7 Point cloud map and octree map storage space

5 结语

本文提出了一种RGB-D ORB-SLAM2的改进稠密建图算法，通过体素滤波和去除离群点滤波算法有效剔除离群杂点，降低点云地图的冗余度；通过稠密回环处理，进一步优化关键帧位姿，提升了点云位姿准确度，加快了建图速度.主要结论如下：

1）试验表明，在同一TUM 数据集下，相比于同样基于ORB-SLAM2 系统的稠密建图算法，本文的改进算法从整体建图质量到局部建图细节都有明显的优化提高，点云冗余度大幅度降低.

2）本文RGB-D ORB-SLAM2系统的绝对轨迹误差与相对位姿误差在静态或低动态均小于RGB-D SLAMV2、DynaSLAM 系统，系统的鲁棒性和漂移性得到提升.

3）为了解决稠密点云地图无法用于移动机器人的导航与路径规划等问题，添加了八叉树地图的构建环节，可降低内存消耗，便于地图的维护和更新优化.