融合全局描述子和半直接法的双目SLAM

梁 鸿，陈灵娜+，何啸峰，周扬帆，蒋良卫

(1.南华大学 计算机学院，湖南 衡阳 421001； 2. 湖南财经工业职业技术学院 电子信息系，湖南 衡阳 421000)

0 引 言

基于视觉的同时定位与地图构建(visual simultaneous localization and mapping，VSLAM)在自动驾驶、移动机器人、增强现实、虚拟现实等新兴技术中发挥着重要作用。回环闭合作为视觉SLAM必不可少的一部分，能确保系统在长期定位与跟踪过程中更加准确；回环闭合最重要的一步是闭环检测。目前，大部分视觉SLAM方案主要使用基于局部特征的视觉Bow模型进行闭环检测。由于可以重用前端视觉里程计的描述子，常被基于间接法的SLAM系统所采用，如流行的ORB-SLAM2[1]、OpenVSLAM[2]都训练了各自的视觉词典用于回环闭合和重定位。Gao等[3]将DSO[4]作为前端视觉里程计，计算关键帧的ORB特征描述子并建立Bow模型，将其扩展为具有回环闭合功能的直接法SLAM。Lee等[5]提出了一种松耦合的半直接法SLAM，亦使用DSO作为前端，并行执行ORB-SLAM2部分功能实现闭环和重定位，并构建全局一致的地图。此类方法不同于间接法SLAM可以重用前端的Bow模型，在使用直接法完成前端位姿跟踪后，需对关键帧提取特征点并计算相应的描述子，增加了计算量。此外，基于局部特征的Bow模型会丢失更高层次的几何和结构信息，不能应对较强的环境变化，且需预先训练视觉词典；相对于局部特征描述子，表示整张图像的全局描述子对外观变化的鲁棒性更好[6]。

本文在借鉴文献[7]的思想上，提出了一种融合全局描述子和半直接法双目SLAM方案—GDSLAM(global descriptor SLAM)。在ORB-SLAM2框架上，融合半直接法进行位姿跟踪，避免了计算特征词向量和特征匹配；利用知识蒸馏的方法训练得到全局描述子计算模型，结合主成分分析(PCA)和K近邻检索，实现快速准确的闭环检测，为系统重定位和回环闭合模块实现了高效准确的定位。

1 系统框架

融合全局描述子和半直接法的双目SLAM框架如图1所示，包括4个部分：双目跟踪模块、局部建图模块、关键帧的全局描述子管理模块、回环闭合模块。位姿跟踪模块通过半直接法实现位姿跟踪。局部建图模块包括构建地图点和维护局部地图，并进行共视图优化。关键帧全局描述子管理模块负责对关键帧计算全局描述子并进行存储和检索。回环闭合模块负责闭环检测和全局优化。其中，在模块内部或者模块之间的双向箭头表示数据的传递和反馈。

图1 GDSLAM系统框架

2 全局描述子的计算与检索

研究表明基于深度学习得到的场景描述在应对环境变化时表现更好[8]，并产生了一些优秀的端到端图像描述模型用于解决大规模的视觉场景识别问题[9]。但是这些模型由于参数太多、计算量太大，不适合用于需要实时计算的SLAM。Sarlin等[7]借助知识蒸馏[10]的方法将大型图像检索模型压缩为具有移动实时推理功能的小型网络，在移动平台上的检索与定位。文献[12]使用学习方式代替传统手工设置的特征提取，通过知识蒸馏实现同时预测局部和全局描述子，实现了类似的分层定位功能，并取得最新的成绩。

2.1 基于MobileNet的全局描述子生成网络

在大型场景识别问题中，基于卷积神经网络(CNN)的研究工作通常分两步生成图像的全局描述子：首先，通过网络编码层计算图像的高级特征图，然后映射到一个紧凑的全局描述子中；最初使用最大池化作为映射层。Arandjelovic等[9]使用局部聚合特征(VLAD)描述方法为 CNN 设计了新的池化层，称为NetVLAD层，更加有效地利用卷积特征，提高了对同类别图像的表达能力和图像区分能力。原始NetVLAD网络由于参数太多，计算量太大，不适合资源受限的平台使用。Sandler等[12]发布了MobileNet，这是一种针对资源受限的设备而设计的编码器体系结构。由于NetVLAD的训练过程很复杂，耗时长，不能满足实时性；本文采取与文献[7]类似的方法，通过知识蒸馏训练一个较小的网络(MobileGDNet)来学习NetVLAD生成的图像全局描述子，这大大减少了新模型的训练时间。通过最小化MobileGDNet与预训练的NetVLAD网络生成的全局描述子之间的均方误差损失(MSE loss)来训练MobileGDNet,训练过程如图2所示。

图2 通过知识蒸馏的方法训练MobileGDNet

2.2 训练与测试

由于原始的NetVLAD网络使用matlab实现，不方便扩展训练；Cieslewski等[13]基于Tensorflow重新实现了NetVLAD(NetVLAD-Tensorflow)，在本实验中也使用基于Tensorflow的NetVLAD作为训练网络模型。MobileGDNet的编码器是MobileNetV2模型，并使用在ImageNet上预训练的权重进行初始化。为了减少计算，从包含较少的通道数(320)的倒数第二个图像编码层计算全局描述子。每个VLAD层具有32个聚类簇，共生成10 240维的图像描述子，该描述子进一步向下映射到4096维，保持与NetVLAD网络相同的输出维数。考虑到许多数据集不是RGB数据而是灰度图，所以网络的输入为640*480的灰度图像，并选择包含各种环境和尺寸的Google Landmarks数据集[14]，从中随机选择10万张图片作为训练数据。实验环境为：操作系统环境为Ubuntu16.04，主频为3.6 GHz的CPU，显存为11 Gb的GTX1080Ti，16 G内存。

为了验证MobileGDNet的图像描述能力，选择包含多个闭环的KITTI数据集00序列作为评估数据。使用主成分分析(PCA)减少描述子的维数，提高检索速度。首先，计算NetVLAD-Tensorflow、MobileGDNet在KITTI-00上的全局描述子，以及通过PCA降维之后的描述子(64维、128维、512维)。然后，计算距离协方差矩阵并获取最相似位置向量。最后，分别与ground truth计算的最相似位置向量进行比较，计算准确率、召回率和AUC值。相应的PR(准确率-召回率)曲线和AUC值，如图3所示。

图3 KITTI-00序列上的PR曲线和相应的AUC值

从图3可见，MobileGDNet生成的描述子与NetVLAD-Tensorflow生成的描述子表现出了相似图像描述能力，而且通过PCA对描述子降维，没有明显削弱其描述能力。从表1可见，通过知识蒸馏，MobileGDNet的模型大小相当于NetVLAD-Tensorflow的1/20，而计算速度提升了6倍以上；此外，在GPU显存为2 G的GTX1050上的计算速度达到了28帧/秒，对于计算资源有限的平台较有利。

表1 模型大小与CPU计算耗时情况

3 融合半直接法的双目SLAM

GDSLAM在ORB-SLAM2框架上，选择具有固定基线的双目相机模型，能够恢复图像点深度信息，解决了单目相机尺度漂移的问题；使用基于光度最小化的半直接法估计位姿，具有更快的位姿跟踪速度。在文中，假设以左相机作为相机坐标系；令π表示相机坐标系下的3D点p到特征点u的投影模型；相应的π-1表示特征点u到相机坐标系3D点p的反投影模型；令Ik-i表示k-i时刻的帧信息，Ik表示当前帧；令Tk,w表示当前帧相机在世界坐标系下的位姿，Tk,k-i表示当前帧与第k-i帧的相对位姿变换。

3.1 融合半直接法的双目位姿跟踪

首先，多线程对矫正之后的双目灰度图提取ORB特征并进行匹配，计算双目特征对的特征点深度值和地图点的3维坐标，完成初始化，此过程与ORB-SLAM2一致。当前帧的相机位姿Tk,w采用匀速运动模式与匹配关键帧两种初始化策略，其中匀速运动模式与ORB-SLAM2一致。直接匹配关键帧初始化相机位姿：基于光度不变假设(即两帧观察到同一个3D点的像素块之间的光度应该相同)，采取从粗到精的迭代顺序进行图像对齐(首先在svo[15]中提出，能够有效地执行两帧间的对齐)；待优化的残差为关键帧Ik-i的特征点u对应的图块与投影到当前帧Ik上图块的光度差。在已知特征点深度du， 相应的最小二乘公式为

(1)

解决该最小二乘问题，一般有前向组合和逆向组合两种算法；其中，逆向组合算法可以提前计算雅可比矩阵J和海塞矩阵H，避免反复计算H而减少计算量；因此，使用逆向组合算法优化该最小二乘模型能够加速运算。通过此方法，得到当前帧与参考关键帧之间的相对变换位姿Tk,k-i， 进而可以得到当前帧的相机位姿Tk,w，Tk,w与帧间相对位姿Tk,k-i的变换公式为

Tk,w=Tk,k-i·Tk-i,w

(2)

通过匀速运动模型或者跟踪关键帧获得初始位姿之后，再与局部地图中的地图点进行匹配，进一步优化当前帧的位姿，待优化的重投影误差如式(3)。实现中使用了开源的图优化算法g2o；由于此处仅优化相机位姿，故也称为Motion-only BA(bundle adjustment)

(3)

3.2 关键帧的全局描述子管理

采取与ORB-SLAM2一致的关键帧判断策略，对于新插入的关键帧，由MobileGDNet计算相应的全局描述子，并通过PCA降维之后加入关键帧的全局描述子存储库中，即建立KD树并添加新的结点；在重定位和回环闭合模块中，结合K近邻检索与半直接法匹配完成闭环检测。

3.3 闭环检测

闭环检测作为视觉SLAM非常重要的一个部分，它的目的是识别以往到过的地方。对于视觉SLAM系统，有效的闭环检测可以帮助系统准确及时地完成回环闭合，极大地减少系统运行过程中的累积误差，从而构建连续的环境地图。此外，闭环检测也可以用来帮助系统在跟踪失败时进行重定位。

回环闭合与重定位相同的地方在于都可以通过闭环检测确定当前帧的大概位置；但重定位在跟踪丢失的情况下，一般在上一帧附近寻找匹配关键帧，确定相机目前的大概位置；而回环闭合为了避免错误的闭环给系统带来严重的影响，在闭环检测过程中，则会排除前不久的关键帧。此外，回环闭合的闭环验证还可以加入更多的逻辑判断，比如相机姿态的空间一致性。不同于ORB-SLAM2通过视觉词袋方法检测候选帧，本文通过关键帧全局描述子管理模块选择重定位和回环检测的候选帧，然后通过位姿匹配验证空间一致性。

3.3.1 重定位

(1)对当前关键帧的灰度图计算全局描述子，并通过K近邻搜索从地图关键帧的描述子存储库中检索得到前几个最相似的关键帧索引，根据关键帧id倒序排列(优先选择当前帧附近的关键帧)。

(2)当前帧使用类似位姿跟踪的方法与检索到的关键帧图像进行位姿对齐，当平均光度误差较小时执行下一步。

(3)在当前帧中搜索来自关键帧的三维点。如果正确匹配的点超过20个，则最小化匹配之间的重新投影误差，并跳过其余关键帧。

只要找到一个有效的关键帧，跟踪就继续以新的估计相机位姿进行。如果重定位无效，则将在接收到下一帧时再次执行。

3.3.2 回环闭合

回环闭合一般分两步执行，首先检测与验证闭环，然后执行位姿图优化与全局BA完成回环闭合。

(1)针对新插入的关键帧，通过K近邻检索前10帧最相似的关键帧；由于回环闭合对候选帧准确性要求更高，需要检测候选帧的连续性，故检索的候选帧数大于重定位的设置值。过滤掉相似性较大的局部共视帧之后，为了加速闭环候选帧检测，只在图像金字塔最高层将当前关键帧与候选帧进行帧间对齐，当光度误差较小时，进行接下来的候选帧连续性检验。

(2)利用位姿图优化减小系统的运动漂移，并执行全局BA，将闭环中所有关键帧的位姿和3D点作为优化变量，通过最小化重投影误差获得全局一致性更好的运动轨迹和环境地图。

4 实验与分析

实验所使用的环境配置:主频为2.4 GHz的四核i5处理器，16 G内存，显存为2 G的GTX1050，系统环境为Ubuntu16.04。此外，选择公开的KITTI数据集和EuRoc数据集[16]验证算法的有效性和准确性，并与当前流行的开源算法ORB-SLAM2、OpenVSLAM进行对比。评估指标使用绝对轨迹误差的均方根误差(absolute trajectory error’s RMSE，ATErmse)表示，绝对轨迹误差是算法的估计位姿和真实位姿的直接差值,反应了算法精度和轨迹全局一致性。为了方便与SLAM框架整合，实验过程中使用tensorflow的C++编译版本部署MobileGDNet；在对已经建好的KD-tree检索时使用了libnabo，主要用于低维空间的快速K最近邻检索。

在回环闭合模块的闭环检测部分，使用轮询匹配所有关键帧的方法代替使用全局描述子检测，实验中以SDSLAM标识，作为对比算法，验证基于全局描述子进行闭环检测在SLAM方案中的及时性和准确性。

4.1 算法加载时间与跟踪帧率

ORB-SLAM2、OpenVSLAM、SDSLAM、GDSLAM这4种算法启动后加载配置参数和词典/模型所用时间的均值和中值见表2。算法的加载时间在实际应用中会一定程度上影响用户的感受。ORB-SLAM2和OpenVSLAM加载了原作者发布的词典，其中ORB-SLAM2加载的词典为文本型，而OpenVSLAM所使用的词典为二进制。从表2中可见，二进制词典相对文本型词典加载时间更短，且占用存储更少。GDSLAM加载的模型较小，时间较短。采用最小化光度误差作为位姿跟踪的SDSLAM加载时间最短。

表2 4种算法加载时间的均值和中值/s

位姿跟踪模块是实时执行的，ORB-SLAM2、OpenVSLAM、GDSLAM这3种算法在KITTI数据集03序列上的跟踪频率如图4所示，该序列共有800帧双目图像；在00-10序列上的平均跟踪频率如图5所示。从图4和图5中可见，GDSLAM平均跟踪耗时较ORB-SLAM2和OpenVSLAM更少，主要原因是：GDSLAM不需要计算图像帧的词向量，并且使用最小化光度误差的半直接法匹配代替特征点匹配。

图4 3种算法在KITTI数据集03序列上的跟踪频率

图5 3种算法在KITTI数据集上的跟踪频率(FPS)

4.2 SDSLAM与GDSLAM的比较分析

SDSLAM与GDSLAM在KITTI数据集各序列上的绝对轨迹误差ATErmse，如图6所示。在00序列上的轨迹如图7所示，该序列共有4541帧双目图像，并包含多个闭环，很适合用来做回环闭合模块的检测。从图6可见，GDSLAM相对于SDSLAM，在大部分序列上都得到更低的误差值，尤其在包含回环的序列(如：00、02、05、06、07、08、09)，GDSLAM的绝对轨迹误差ATErmse都比SDSLAM低许多。从图7可见，GDSLAM能够很好检测到各处的回环闭合点，而SDSLAM在最后的回环闭合处不能准确识别并减少累计误差。可以验证，在回环闭合模块中，基于全局描述子的检测方法相对于使用直接法匹配更加及时且准确。

图6 SDSLAM和GDSLAM在KITTI数据集下的 绝对轨迹误差ATErmse

图7 SDSLAM和GDSLAM在KITTI数据集的 00序列上的轨迹

4.3 KITTI和Euroc数据集

ORB-SLAM2、OpenVSLAM、SDSLAM、GDSLAM这4种算法在公开的双目数据集KITTI和Euroc上的实验结果见表3。从表3中可见，ORBSLAM2和OpenVSLAM作为当前流行且开源的间接法SLAM在两个数据集上的表现相差不大。本文的GDSLAM在KITTI数据集的03、05、06、09序列上误差最小，其中05、06、09序列只有一个闭环，且都处于图像序列的结尾部分，对闭环检测的及时性要求较高(在图像序列处理完之前，若不能及时检测到闭环，就不能减少累积误差，实验误差较大)；GDSLAM在09序列上的误差相对ORB-SLAM2和OpenVSLAM分别减少了57%和32%；由此也验证了GDSLAM闭环检测的及时性和准确性。

从表3中也可见，GDSLAM在Euroc数据集的V101、V201、MH05序列上误差最小。SDSLAM和GDSLAM在Euroc数据集上表现相当，而在KITTI数据集上，SDSLAM表现较差。由于KITTI数据集为室外环境且帧间的运动较大，要求算法能够及时且准确地检测到闭环并减少累积误差。Euroc数据集为室内环境，算法中回环闭合模块对实验结果的影响较小。

从以上实验可见，本文提出的GDSLAM相对于两种流行的间接法SLAM(ORBSLAM2、OpenVSLAM)在跟踪耗时方面有一定的优势；相对于在回环闭合模块中采用遍历所有关键帧的方式进行闭环检测(如本文所设计的SDSLAM)，GDSLAM能够及时准确地检测闭环候选帧，完成回环闭合并减少累计误差。

5 结束语

为了有效地解决基于半直接法视觉SLAM方案的闭环检测问题，提出了融合全局描述子和半直接法的双目SLAM(GDSLAM)。在ORB-SLAM2框架上，使用直接法代替基于ORB特征匹配，避免计算特征词向量，一定程度上提高了跟踪帧率；通过知识蒸馏，训练得到图像的全局描述子生成模型，减少了训练时间且提高了已有网络的通用性。构建关键帧的全局描述子管理模块，在GDSLAM的重定位和回环闭合模块中能够快速准确地完成闭环检测。在两个公开数据集KITTI和Euroc上进行实验，并且与流行的开源算法ORB-SLAM2、OpenVSLAM进行对比；实验结果表明，相对于SDSLAM(遍历所有关键帧进行闭环检测)，GDSLAM能够更及时地检测到闭环，得到全局一致性更好的轨迹和地图；在KITTI数据集中包含闭环的序列上，GDSLAM在其中大部分序列上取得了更小的绝对轨迹误差；同时，在取得相似跟踪精度的情况下，能够得到更高的跟踪帧率。在接下来的工作中，进一步提高前端跟踪帧率，融入更高层图像信息(如图像语义信息等)，设计鲁棒性更好的全局描述子生成网络。

表3 4种算法在KITTI和Euroc数据集下的绝对轨迹误差ATErmse