基于地图的RGB-D SLAM系统

准确的基于地图的RGB‐DSLAM用于移动机器人

多米尼克·贝尔特、米哈尔·诺维茨基和皮奥特尔·斯克日普钦斯基

1引言

某些类型的机器人，比如四旋翼[19]和足式机器[2, 17]，需要在三维空间中进行精确的位姿估计以实现可靠的导航。最近，在这些机器人平台上，采用紧凑型 RGB‐D传感器（如Kinect或Xtion）解决SLAM问题的方案变得流行起来。使用稠密深度数据的方法，例如Kintinuous[25],，虽然表现出高精度，但需要高端通用图形处理器上的硬件加速，而这无法在小型移动机器人上实现机载集成。一种可行的替代方案是基于位姿的RGB‐DSLAM方法，该方法通过计算连续帧之间的传感器运动来估计

D.贝尔特 · M.诺维茨基 · P.斯克日普钦斯基´(B)波兰波兹南理工大学控制与信息工程研究所，皮奥特罗沃街3A，60‐965波兹南，波兰电子邮件： {dominik.belter,michal.nowicki,piotr.skrzypczynski}@put.poznan.pl ©施普林格国际出版（瑞士）2016L.P.Reis等编，《2015机器人：第二届伊比利亚机器人会议》，《智能系统与计算进展》
418，DOI:10.1007/978‐3‐319‐27149‐1_41
533
本文档由funstory.ai的开源PDF翻译库BabelDOCv0.5.10(http://yadt.io)翻译，本仓库正在积极的建设当中，欢迎star和关注。
534 D.Belter等人
轨迹。所获得的传感器位姿和运动相关约束分别作为因子图的顶点和边，然后进行优化[14]。每当机器人返回已访问过的位置时检测到的回环闭合会引入位姿对位姿约束，从而使SLAM中的图优化能够校正轨迹漂移。两帧之间的传感器运动估计可以通过多种方式获得，例如应用稠密光流[12] 或稀疏光流[16],，但最常见的方法是使用局部视觉描述子匹配稀疏特征[6, 11]。

正如我们在[3] 中所展示的，基于位姿的RGB‐DSLAM方法能够在没有硬件加速的情况下实现准确的实时轨迹估计。然而，该方法忽略了由特征频繁重观测所产生的大量特征到位姿约束。由于回环检测在位置数量上具有线性复杂度，在基于位姿的SLAM中，数据关联通常仅在沿轨迹记忆的关键帧中相对较小的一部分之间建立。

相比之下，在现代视觉SLAM广泛采用的捆绑调整（BA）方法[23],中，特征到位姿约束直接用于优化。Henryetal.[11]已将两视图BA应用于改进 RGB‐D视觉里程计（VO）中的帧间运动估计，而舍勒和泽尔[19]提出了一种基于并行跟踪与建图结构（PTAM）[13]的RGB‐DSLAM解决方案。文献 [15]提出了在子地图上定义局部BA以从RGB‐D数据进行3D物体重建的方法。

最近，SlamDunk系统被提出[7],，该系统使用点特征池（地图）来跟踪传感器运动，并将这些特征纳入优化过程。

本文研究了如何高效实现一种基于因子图优化并包含三维特征的 RGB‐DSLAM。此类解决方案的关键组件包括快速准确的传感器跟踪、地图维护，以及地图与感知到的特征之间的鲁棒数据关联。本工作提出了一种新颖的基于地图的RGB‐DSLAM系统架构，该架构与其他类似方法 [7, 15, 19]的不同之处在于：利用视觉里程计流程跟踪传感器位姿，定义具有缓冲部分的因子图灵活结构以实现后台实时优化，并探索了数据关联中离群值剔除的新技术。实验评估重点分析了视觉里程计流程实现对轨迹估计精度的影响，以及地图中特征的行为。此外，还包含了与若干当前主流 RGB‐DSLAM方法的比较。

2系统架构

2.1RGB-D数据处理

我们的RGB‐DSLAM系统1的实现分为前端，用于实现VOpipeline和数据关联过程，以及

1源代码可在https://github.com/LRMPUT/PUTSLAM/tree/release获取

用于移动机器人的准确基于地图的RGB‐DSLAM 535

后端实现地图结构和因子图优化。该架构如图1所示。

前端的主要任务是估计局部传感器运动，并将观测到的点特征与现有地图进行关联。在前端中，传感器位移估计来自视觉里程计流程（VO pipeline），该流程通过3D到3D特征对应关系实现帧间运动估计，类似于立体视觉[17]。三维特征是通过将从RGB图像中提取的2D特征（关键点）结合现成的深度数据增强而获得的。2D关键点之间的对应关系可以通过视觉描述符匹配或光流跟踪来完成。基于匹配的方法结合局部描述符在 SLAM研究中较为流行[8]。我们在此展示的另一种方法仅在由n帧组成的序列的第一幅RGB图像中检测关键点（n较小），然后使用Lucas‐Kanade算法[20]在接下来的n幅图像中跟踪这些特征。该变体提供了一个非常快速且相当准确的视觉里程计流程[16]。

一旦建立了两个RGB‐D帧之间的对应关系，便使用宇山算法[24]来估计这两个帧之间的SE(3)刚性变换。该估计过程嵌入在RANSAC框架中，以使计算出的变换对因跟踪不准确或描述符关联错误而产生的离群点具有鲁棒性。

RANSAC返回与最多特征点对一致的变换，这些特征点对被视为内点。最终的变换使用g2o优化库对内点特征集执行两视图BA来计算。

所得的传感器运动估计会首尾相连地添加到因子图中最后一个已知的传感器位姿，以计算最后一个RGB‐D帧的姿态估计。利用来自VO的这一初始位姿猜测，从地图中选取的特征将成为与当前局部特征集进行匹配的候选特征。当前

536 D.Belter等人

特征集由在最近n帧中被跟踪的点或在当前RGB‐D数据帧中检测到的点组成，前提是使用基于匹配的视觉里程计。在本文展示的实现中，我们在基于跟踪的流水线中采用ORB特征[18]，在基于匹配的版本中采用SURF[1]。为了提高计算效率，视觉里程计中所用特征的描述符会被重新用于获取局部特征与地图之间的关联。Lucas‐Kanade跟踪器所使用的特征必须是角点状的，因此选择ORB。基于匹配的版本采用SURF，其具有斑点状检测器，但提供了区分性更强的描述符。

无论使用何种特征类型，为了实现对传感器运动的良好估计，都有必要将关键点分布在图像帧的所有区域。因此，RGB图像被划分为 80×80像素的方形子图像，并在每个子图像中单独进行特征检测，同时调整检测器参数，以确保每个方块中获得大致相同数量的特征。特征检测完成后，使用 DBSCAN无监督聚类算法来检测图像中的关键点组，然后每组仅由一个最强关键点[16]表示。

在获得传感器位移估计后，前端通过使用视觉描述符（ORB或SURF）将局部特征与地图中已存储的特征进行关联。基于这些关联，前端通过对对应特征对应用宇山算法，计算当前姿态与地图（在全局坐标系中）之间的 SE(3) 变换。同样，内点由RANSAC确定。对于内点集合 R3，在当前帧的姿态与地图中重新观测到的特征之间添加约束（平移）。

2.2地图优化

后端维护地图并构建因子图，然后使用g2o库[14]进行优化。地图包含一组由视觉描述符增强的3D点特征，以及与关键帧相关的传感器位姿。这些特征通过约束与位姿相连 R 3。位姿以全局参考坐标系表示，而特征则锚定在首次观测到它们的位姿的坐标系中。传感器位姿由其笛卡尔坐标位置（x，y，z）和表示传感器方向的四元数（qw，qx，qy，qz）表示。特征pf j(j = 1… m)表示为3D点。由于图优化的计算复杂度与变量数量呈三次方关系，因此在因子图中引入大量特征的系统的可扩展性受到限制。为此，我们的架构采用双窗口优化框架[21]，在因子图中定义围绕当前帧的内窗口关键帧和外围关键帧的外窗口。内窗口之外的特征图部分被边缘化，从而使因子图优化更加高效。

面向移动机器人的准确基于地图的RGB‐DSLAM 537

然而，图优化过程仍然比前端慢得多。因此，前端和后端像PTAM架构 [13]一样实现在独立的线程中。前端和后端异步工作，并在特定事件（例如对可见特征的查询或插入一组新约束）时进行同步。新的位姿、特征和测量值被缓存在一个较小的临时图中，在后端完成正在进行的图优化会话之前，该临时图不会被g2o使用。此时，临时结构将与主图合并。

与典型的视觉SLAM系统不同，我们的算法不是直接在地图上跟踪相机，而是采用一个高效的视觉里程计流程来预测传感器位置。这使得我们的SLAM系统能够在当前视图与地图之间重叠非常小、且无法重新观测到已映射特征的情况下正常工作。在这种情况下，会将直接的位姿对位姿约束引入因子图，以稳定g2o中的优化。

通过最小化以下函数来求得最优传感器位姿pc 1,…, pc n和特征位置pf 1,… p mf：

argmin
p
F=
n
∑
i=1
m
∑
j=1 e(pc i, p f j, mij) Tij e(pc i, p f j, mij), (1)

其中e(pc i,p f j,mij)是估计位姿与测量值之间关于测量mij对应的顶点的误差函数。在我们的公式中，mij对于特征到位姿约束为tij ∈ R 3，对于位姿对位姿约束为Tij ∈ SE(3)。信息矩阵 表示每次测量的精度。对于特征到位姿约束，该矩阵通过对特征不确定性的协方差矩阵求逆得到。在本研究中，我们假设特征具有各向同性空间不确定性，因此将设为单位矩阵，但正如我们最近的研究所示[4]，通过将计算为真实特征协方差矩阵的逆矩阵，可以提高轨迹估计的精度。位姿对位姿约束的同样被设为单位矩阵，但它也可以根据在计算两个位姿之间变换时被边缘化的测量值的不确定性进行估计 [9]。

3地图中的数据关联

我们的系统通过在地图中建立特征到位姿的约束，隐式地处理局部的、度量的回环闭合。该地图包含在传感器轨迹上不同时刻发现的特征。仅考虑那些锚定在当前传感器位姿欧几里得邻域内的传感器位姿上的特征进行匹配。在当前实现中，系统未考虑需要纯粹基于外观的地点识别的全局回环闭合[5]。然而，此类方法的集成被视为未来的工作。

前端向后端（持有地图数据结构）查询一组候选特征，这些特征可能从传感器的当前姿态被重新观测到。

538 D.贝尔特等人

通过该查询得到的特征集被投影到当前帧中。如果特征在图像上的投影位置是可接受位置，则考虑进行匹配。根据引导匹配原则，候选匹配仅在图像平面上预测特征的小邻域内考虑。由于地图中的点特征通常从不同视角被观测到，因此我们为地图中的单个点特征存储了在不同观测角度获取的多个 ORB/SURF描述符。在进行特征匹配时，我们选择其存储的观测角度与当前帧的观测角度差异最小的描述符。观测角度在全局坐标系中表示，并通过点积快速计算它们之间的夹角。当映射特征的最相似视角与当前传感器视角之间的观测角度差异大于 30◦时，我们完全拒绝这些匹配。

当我们尝试将局部特征与地图进行关联时，重要的是要限制用于匹配的地图特征数量。因此，我们会剔除从当前姿态观察时被遮挡的特征。这通过当前帧中readilyavailable的稠密深度测量来实现。我们将每个从地图投影到当前坐标系并投影到图像平面的特征的距离（即特征的“预测深度”）与在地图特征的投影图像坐标周围测得的平均深度进行比较。如果测量深度比预测深度短超过0.1米，则认为该特征被遮挡（图2a）。这种方法比仅考虑从局部邻域可见特征的方法更加精确[21]（图2b）。

通过描述符匹配确定的视觉对应关系集合中，使用抢占式RANSAC框架估计正确匹配（内点）。RANSAC中的外点剔除测试可以采用图像平面中的特征重投影误差，或三维空间中观测特征与预测特征之间的欧几里得距离。重投影误差定义为：

ERep.= max(d2D(a(p f i),a(p f j )), d2D(b(p f i),b(p f j ))), (2)

面向移动机器人的准确基于地图的RGB‐DSLAM 539

其中 a表示投影到图像平面的算子a，d2D(x,y)表示两个图像点x和y之间的图像距离计算，而pf i是第i个特征的三维位置。上述特征之间的替代欧几里得范数计算如下：

EEuc.= d3D(pf i, pf j), (3)

其中d3D(x, y)是计算两个3D点x和y之间的欧几里得距离。我们实现了两种离群值剔除方法，以测试在实际应用中哪种方法更高效。

4地图维护

如果在当前关键帧中发现的点特征无法与已映射特征匹配，则将它们添加到地图中。在添加特征时，我们试图避免附近特征可能产生的混叠，这可能导致错误匹配。因此，我们仅从对环境探索有足够贡献的关键帧中添加特征至地图。当从地图重投影到当前图像中的特征数量低于给定阈值（通常设为 80），或地图匹配过程产生的特征到位姿约束少于预设阈值（通常设为25）时，便从输入的RGB‐D数据流中选择一个关键帧。每个关键帧最多可向地图添加40个点特征，并优先选择检测器响应最强的关键点。位于离传感器太近或太远的特征不会被添加到地图中，因为由于测距误差，它们的位置可能具有高度不确定性。

当向地图中添加一个新特征时，我们还会验证该新点与地图中已存在的特征之间的距离是否足够远。在欧几里得空间中存在一个最小距离阈值，以及在图像平面上的最小距离阈值。然而，为了获得最佳精度，这些参数通常需要根据场景或传感器运动的不同特性进行调整。

5评估与结果

5.1地图演化的研究

我们在ICL‐NUIM数据集上进行了一系列实验[10]，以了解估计轨迹的精度如何受到基于视觉里程计的运动估计、RANSAC中误差度量的选择以及地图中特征测量值分布等因素的影响。选择在合成环境中渲染的ICL‐NUIM数据集，是因其提供了完美的真实传感器轨迹，并且该数据集提供了两种深度数据变体：一种假设无噪声，另一种包含模拟噪声。我们研究了

540 D.Belter等人

地图中特征的演化，并使用广泛采用的ATE（绝对轨迹误差）和RPE（相对位姿误差）度量[22]对定量结果进行了评估。

图3显示了从示例office_room/kt2序列中恢复的轨迹。基于跟踪的版本估计的轨迹（图3a）比基于匹配的变体估计的轨迹（图3b）略欠准确。这些示例轨迹是从假设无噪声的数据中获得的，然而，鲁棒特征匹配和外点剔除机制使得我们的SLAM在该序列的含噪声版本上也能获得类似的结果（图3c）。

表1 我们的SLAM系统在ICL‐NUIM办公室_房间/kt2序列上，无深度噪声和有深度噪声情况下的代表性配置性能

表1总结了基于跟踪和基于匹配的SLAM版本在office_room/kt2序列上获得的定量结果，并展示了RANSAC中的外点剔除指标如何影响所获得轨迹的精度。所有帧率（FPS）均在配备IntelCorei7‐26003.4GHzCPU和 16GB内存的PC上测得。图4a展示了同一序列的基于特征的地图可视化2。基于匹配的版本实现了更高的精度（更小的ATE），但代价是速度显著降低。

RANSAC中误差指标的影响较小，但重投影误差指标始终能提供略高的精度。然而，公式（2）中更复杂的计算会减慢前端的速度。

2一个短视频片段可在http://lrm.cie.put.poznan.pl/robot15.mp4获取

面向移动机器人的准确基于地图的RGB‐DSLAM 541

将表1中的ATE均方根误差结果与[10]中给出的五个SLAM或视觉里程计系统的数值进行比较，可以看出，我们的基于匹配的SLAM比在_officeroom/kt2序列上评估的所有基于特征的方法都更准确。只有基于稠密 ICP的Kintinuous在此序列上实现了更小的ATE，但它需要硬件加速才能运行。即使是基于跟踪的版本，虽然能够以高帧率运行，仍可使传感器定位的位置误差约为4.5厘米（含噪声）。

为了更详细地研究地图中特征的行为，我们进行了一项实验，展示了所有测量特征在公共坐标系中的分布情况。我们在相同的office_room/kt2序列上运行我们的SLAM算法。每当算法计算出两个帧之间的变换时，我们用真实值中已知的精确变换替代计算得到的变换。因此，特征位置的误差仅由前端中的特征检测、跟踪或匹配不完美引起。然后，我们使用真实值数据将所有测量到的特征位置(u v)变换到第一幅RGB图像中。图4b和4c展示了该RGB图像中一个选定的、富含特征区域的放大视图。基于跟踪的前端为每个特征提供了更多的测量值，这对因子图优化是有利的。然而，当前端对单个特征进行长时间的连续跟踪时，测量值开始产生漂移。这些特征在图像上形成椭圆形（图4b）。由基于匹配的前端生成的特征在图像（图4c）以及欧几里得空间中分布更为分散。基于匹配的版本在每一帧都重新建立特征，因此在依据描述符进行特征到地图的关联时具有更强的判别能力。因此，一旦某个特征开始漂移，基于匹配的前端就会创建一个新的特征。这种方法在 SLAM中实现了更高的精度（特征漂移更小），但最终导致特征彼此靠近，地图迅速扩展。大量的特征加上相对较慢的SURF检测器/描述符，导致基于匹配的版本帧率较低（参见表1）。

5.2基准数据上的精度评估

我们在TUMRGB‐D基准上评估了我们基于地图的RGB‐DSLAM的精度，该基准已被广泛用于展示许多RGB‐DSLAM和视觉里程计系统的性能。我们在五个代表室内导航的序列上测试了我们的SLAM系统：fr1_desk（571帧）、 fr1_desk2（611帧）、fr1_room（1352帧）、fr2_desk（2217帧）以及 fr3_long_office_household（2486帧）。

表2 在五个TUMRGB‐D基准序列上测量的SLAM系统两种主要配置的位姿ATE和RPE

这些序列上我们的SLAM系统采用两种前端变体所达到的位置ATE均方根误差和位置RPE均方根误差结果总结于表2（注意fr3_office代表fr3_long_office_household）。在这些测试中，一些显著依赖数据特性的参数（如地图维护过程中的距离阈值）已针对每个序列单独调整。对于全部五个序列，基于匹配的版本比基于跟踪的版本更准确。在TUM序列中，存在大量带有明显运动模糊的RGB帧，而使用类角点ORB检测器的基于跟踪的前端在这些帧中出现了漂移以及关键点可能的混叠问题。基于匹配的版本通过多尺度SURF生成了更具重复性的点特征，从而实现了非常准确的位姿估计。

然而，这以低帧率为代价，其帧率比基于跟踪的版本低一个数量级。基于跟踪的版本达到了对于大多数移动机器人任务而言令人满意的轨迹估计精度，并且能够在没有任何硬件加速的情况下实时运行。作为比较，我们在表2中还提供了其他最先进的系统在相应序列上取得的位置ATE均方根误差值，即 RGB‐DSLAM[6]以及文献[15]中描述的基于子图的BA。据我们所知，从文献分析来看，这些是截至目前在相应TUM基准序列上发表的最佳ATE结果。我们的方法取得了相似甚至更小的位置ATE值。三个典型序列恢复出的轨迹如图5所示。

、fr2_desk(c,d)和fr1_desk(e,f)，由我们的SLAM系统通过基于跟踪的(a,c,e)和基于匹配的(b,d,f)前端恢复)

6结论

我们提出了一种新的基于特征的RGB‐DSLAM系统，该系统采用因子图优化方法，但与更流行的基于位姿的SLAM方法不同，它构建了一个持久性的三维特征地图。本系统中的新概念是使用视觉里程计流程来计算传感器运动估计，而不是直接相对于特征地图进行传感器跟踪。通过使用非常快速的稀疏光流跟踪器实现这一概念，使得该基于地图的RGB‐DSLAM能够在普通 PC上以Kinect帧率运行，且无需任何硬件加速。我们的算法建立并维持了大量的特征到位姿约束，从而实现了比基于位姿的SLAM系统更精确的轨迹重建。通过新的特征管理技术提高了鲁棒性：利用稠密深度图像直接检测被遮挡特征，并为已映射特征关联多视角描述符。基于跟踪的前端实现了实时操作（最高达38Hz），但在轨迹估计精度上低于用于比较的基于匹配的变体。这归因于特征类型的选择：ORB关键点重复性较差，且更容易因运动模糊而产生误差。因此，跟踪特征的漂移程度大于SURF生成的点。SURF关键点在图像上的分布更加均匀且稳定，但SURF计算和匹配速度过慢，无法满足实时操作的需求。