LDSO: Direct Sparse Odometry with Loop Closure（翻译）

准备把高博最新的论文LDSO翻译下来：

原文

摘要—

在本文中，我们提出直接视觉里程计（DSO）[1]到单目视觉SLAM具有闭环检测和姿势图优化的系统（LDSO）。作为直接法，DSO可以利用任何具有足够的强度梯度的图像像素，使其在缺少特征点的区域仍具有很强的鲁棒性。 LDSO保持这种鲁棒性，而同时确保在前端跟踪中检测到的特征角点的可重复性。这种可重复性保证了传统的基于特征的词袋（BoW）的闭环检测方法的可靠性。闭环回路检测部分在几何上被验证，并且通过联合最小化2D和3D几何误差项来估计Sim(3)相对姿势约束。这些约束与从DSO的滑动窗口优化中提取的相对姿势的共同部分进行融合。我们对公开数据集的运行结果表明，修改后的选择点策略保留了跟踪准确性和鲁棒性，并且集成的姿势图优化显著的减少了旋转，平移和比例漂移误差的累积，从而使整体性能与最先进的基于特征的系统相媲美，即使没有全局BA优化。

I.引言—

自20世纪80年代以来，同步定位和映射（SLAM）已成为计算机视觉和机器人技术领域的一个活跃研究领域[2]，[3]。 它是许多需要实时定位的应用的基础模块，，如移动机器人，自动MAV，自动驾驶，以及虚拟和增强现实[4]。 虽然其他传感器模式如激光扫描仪，GPS或惯性传感器也常用，但视觉SLAM非常受欢迎，部分原因是因为相机在消费产品中很容易获得并被动的获取有关环境的丰富信息。 特别的，在这项工作中，我们专注于跟踪单个单目灰度相机的情况。

通常，可视SLAM系统由摄像机跟踪前端和后端组成，后端创建并维护关键帧映射，并通过循环闭包检测和地图优化减少全局漂移。 前端可以根据当前地图[4]，[5]全局定位摄像机，使用视觉（关键帧）测距法（VO）[6]，[7]在本地跟踪摄像机，或者使用两者的组合[8]， [9]，[10]。

在自适应直接滑动窗口边缘化里程计系统（如DSO）以重用地图中的现有信息时，存在若干开放性挑战。 例如，为了评估光度误差，必须将过去关键帧的图像保存在内存中，并且当结合来自先前关键帧的测量时，确保评估的一致性是具有挑战性的，因为来自这些关键帧的信息已经包含在边缘化中。 之前不应该重复使用。 因此，我们建议将DSO作为我们的SLAM前端，以估计具有局部一致性的视觉里程计并通过后端闭环检测和姿势图优化来校正其漂移。 请注意，DSO本身也包含一个摄像机跟踪前端和一个优化关键帧和点深度的后端。但是，在这项工作中，我们将整个DSO称为我们的里程计前端。

VO方法可以分为两类：间接（基于特征）方法，通过固定的，先前估计的可重复离散特征点之间的对应关系来最小化重投影误差，以及对用的直接法是通过最小化直接图像中的光度误差来联合估计运动和对应关系。虽然基于特征的方法长期以来一直是主流，但直接VO的最新进展已经表现出更好的准确性和鲁棒性，尤其是当图像不包含足够的明确角点特征时[1]，[11]。直接方法的稳健性来自运动和对应的联合估计以及还使用对应于边缘或甚至平滑图像区域的非角落像素的能力（只要存在足够的图像梯度）。然而，在没有闭环的情况下，间接和直接VO都受到不可观察的自由度的累积漂移的影响，这是单目情况下的全局平移，旋转和缩放。这使得长期相机轨迹和地图不准确，从而将应用限制为仅短期运动估计。

为了闭环，需要首先检测它们。最先进的环路检测方法 - 有时称为appearence-only SLAM ，通常基于索引图像特征（例如BoW [12]，[13] ]，[14]，[15]）因此可以通过重用前端的特征直接应用于基于特征的VO。然而，这在直接法中并不那么简单：如果我们独立于前端检测和匹配特征，我们可能没有这些点的深度估计，我们需要有效地估计Sim（3）姿势约束，如果相反，我们尝试重用前端的点和计算描述符，它们可能与可重复的特征不对应，导致环路闭合检测不良。这里的关键见解是直接VO并不关心所选（或跟踪）像素的可重复性。因此，直接VO系统过去已经通过仅使用关键帧接近来进行闭环检测[6]或者通过独立于前端跟踪和约束计算来计算闭环检测的特征来扩展到SLAM [7]。然后使用直接图像对齐来估计相对姿势约束[6]，[7]，这需要关键帧的图像保持可用。我们建议将关键点选择转向可重复的特征，并使用几何原理来估计约束。总之，我们的贡献是：

***我们采用DSO的点选择策略来实现选取可重复的角点功能，同时保持其对特征点贫乏的环境的鲁棒性。 然后，使用传统的BoW将所选的角点特征用于环路闭合检测。

***我们利用匹配特征点的深度估计来计算Sim（3）姿势约束，结合仅姿势束调整和点云对齐，并且 - 与前端里程计并行 - 将它们与从DSO的滑动窗口优化中提取的相对姿势进行共同可见性图表融合 。

***我们在公开可用的真实世界数据集上演示了点选择保留了跟踪前端的准确性和稳健性，并且姿势图优化显着减少了里程计的漂移并且使得整体性能与最先进的基于特征的方法相当，即使没有全局BA优化。

***我们将成果进行了公开了。

Fg1  说明LDSO在TUM-Mono数据集中如何通过闭环校正累积漂移误差  （ 在TUM-Mono数据集中有和没有闭环的估计轨迹。 左侧部分是来自序列31的样本图像，其终点应与起点位于同一位置。 右侧部分显示了LDSO在闭环前（红色）和闭环后（黄色）的估计轨迹。放大突出显示了轨迹和点云在对齐之后如何更好地对齐。

Ⅱ.相关工作—

近年来已经出现了许多成熟的基于特征的单目SLAM系统，这些系统通常受到开创性PTAM的启发[4]，其最初提出将系统拆分为像机跟踪前端和基于优化的映射后端，后来ScaViSLAM [16] ]，建议将局部束调整与Sim（3）姿势图优化混合。 其中一个更有影响力的系统是ORB-SLAM [5]。 它具有多层次的地图优化，从关键帧插入后的局部束调整开始，在使用BoW检测到闭环后的全局姿势图优化，以及最后（昂贵的）全局BA优化。 由于与LDSO不同，它使用传统的特征匹配来针对当前地图定位图像，因此通过删除不需要的关键帧和未使用的特征来强调地图维护。 请注意，尽管LDSO中的闭环检测和姿势图优化是类似的，但我们只需要为关键帧计算特征描述子。

视觉里程计已经以不同方式扩展到SLAM系统。值得注意的是，许多系统建议集成惯性传感器[17]，[9]，[8]，[10]和/或使用立体声设置[17]，[10]，[18]，[19]因为这可以增加在具有挑战性的环境中的鲁棒性并且可以观察到额外的自由度（全局尺度，滚动和俯仰）。 Lynen等人. [8]建议直接包括来自现有地图的2D-3D匹配作为卡尔曼滤波器更新在本地MSCKF样式的测距估计器中。Okvis [17]是一个基于特征的视觉 - 惯性关键帧里程计，它在一个恒定大小的边缘化窗口（类似于DSO）中维护一个特征点的局部地图，一旦它离开本地窗口就不再重用。之后，在maplab [10]中采用了类似的视觉 - 惯性加权最小二乘优化策略，用于批量地图优化而没有边缘化，但作为相机跟踪前端，使用基于卡尔曼滤波器的估计器来最小化跟踪图像块之间的光度误差。基于特征的现有地图本地化作为姿势更新包含在前端中。 VINS-Mono [9]又是一种基于特征的单目视觉惯性SLAM系统，采用了与okvis非常相似的边缘化测距前端。它包括此滑动窗口优化中现有地图的特征观察。与ORB-SLAM和我们的工作类似，闭环检测和全局地图基于BoW和姿势图优化，但在惯性传感器的帮助下，使用非旋转不变的简要描述符并在4自由度中进行图形优化就足够了。在ORB-SLAM中，特征提取步骤几乎占运行时间的一半，VINS-Mono中的前端跟踪基于KLT功能，因此能够在低成本嵌入式系统上实时运行。然而，这意味着，对于闭环检测，必须为关键帧计算附加特征点和描述子。

作为直接单目SLAM系统和DSO的前身，LSD-SLAM [7]采用FAB-MAP [15] 一种仅外观的环路检测算法 - 来提出大环闭合的候选匹配。 但是，FAB-MAP需要提取自己的特征并且不能重复使用来自VO前端的任何信息，并且约束计算反过来不会重复使用特征匹配，而是依赖于使用半稠密深度的直接图像对齐 两个方向上的候选帧的映射和统计测试以验证循环闭合的有效性，这也意味着需要保持所有先前关键帧的图像可用。

Ⅲ.在DSO中的闭环—

A.主干

在深入研究闭环线程如何工作的细节之前，我们首先简要介绍DSO的一般框架和表述。 DSO是一种基于关键帧的滑动窗口方法，其中维护5-7个关键帧，并在当前窗口中共同优化它们的参数。令为关键帧位姿以及在滑动窗口中n个点（逆深度参数）。要最小化的光度误差定义为

是的邻域模式，a，b是仿射光变换参数。t是曝光时间。I表示一幅图像，是一种启发式加权因子

是I上p的重新投影的像素，计算公式如下：

反投影功能

R和t冲两个帧之间的相对刚体运动计算得出Ti和Tj，d是该点的逆深度。

当新帧到达时，DSO通过将当前窗口中的所有活动3D点投影到该帧中来使用直接图像对齐来估计其初始姿势。 如果需要，此后的帧将被添加到本地窗口BA优化中。 滑动窗口自然形成像ORB-SLAM一样的共同可见性图形，但是共同可见信息从不在本地窗口之外使用，因为旧的或冗余的关键帧和点被边缘化。 尽管使用先验边缘化，窗口优化在计算上变得轻量且更准确，但估计将不可避免地漂移。

DSO大型闭环需要全局优化线程。理想情况下，应使用光度误差进行全局光束调整，这很好地匹配DSO的原始方法。但是，在这种情况下，需要保存所有图像，因为在图像上计算了光度误差。此外，现在对DSO选择的点进行全局光度束调整仍然是不切实际的。为了避免这些问题，我们转向使用姿势图优化的方法，这给我们带来了其他几个挑战：（i）如何将全局姿态图优化的结果与窗口优化的结果相结合？更进一步，如何使用滑动窗口中的信息设置姿势图约束，考虑到姿势图优化最小化关键帧之间Sim（3）几何误差，而在滑动窗口中我们最小化光度误差？（ii）如何提出循环候选人？虽然循环检测的主流是基于图像描述子，但我们是否应该简单地添加另一个线程来执行那些与功能相关的计算？ （iii）一旦提出循环候选者，我们需要计算它们的相对Sim（3）变换。在直接图像对齐方法中，我们需要设置相对姿势的初始猜测以启动Gauss-Newton或Levenberg-Marquardt迭代，这在这种情况下具有挑战性，因为相对运动可能远离路标点。

考虑到这些挑战，我们设计了闭环模块，如图2所示。除了DSO窗口，我们还添加了一个全局姿势图来维护关键帧之间的连接。 DSO的滑动窗口自然形成一个共同可见性图形，我们可以将关键帧之间的相对3D姿势变换作为成对姿势测量。 对于环路检测和验证，我们依靠BoW并提出一种将ORB特征与DSO的原始采样点相结合的新方法。 以这种方式，如果提出并验证了循环候选，则计算其关于当前关键帧的Sim（3）约束并将其添加到全局姿势图，其随后被优化以获得更准确的长期相机姿态估计。

B.具有可重复特征的点选择

值得注意的是，即使在像LSD-SLAM或DSO这样的直接法中，仍然需要选择先点; 与间接方法的一个区别是直接方法不需要这些点的可重复性。 即使在弱纹理环境中，DSO也使用动态网格搜索来挑选足够的像素。 我们修改此策略以使其对角落更敏感。 更具体地说，我们仍然选择一定数量的像素（在DSO中默认为2000），其中一部分是角落（通过使用易于计算的Shi-Tomasi分数[20]检测到），而其他像素仍然是 使用为DSO建议的方法选择。 保持角数小，我们计算它们的ORB描述符[21]并将它们打包成BoW。 VO前端使用角落和非角落进行相机跟踪，从而保持将闭环线程的特征提取降至最低的资源消耗。

图3显示了原始DSO和LDSO中的像素选择。 可以看出，由DSO选取的像素具有很小的可重复性，因此难以使用这些点来寻找闭环的图像匹配。 在LDSO中，我们使用角落和其他具有高梯度的像素，其中角落用于构建BoW模型和用于跟踪，而非角落仅用于跟踪。这样我们可以一方面跟踪弱纹理 另一方面，如果需要，也可以匹配关键帧之间的功能。

C.闭环匹配点选择与检查

当我们为每个关键帧计算ORB描述符时，使用DBoW3构建BoW数据库[14]。 通过查询数据库为当前关键帧提出闭环匹配点，并且我们仅选择当前窗口之外的那些（即，边缘化关键帧）。 对于每个匹配点，我们尝试将其ORB特征与当前关键帧的ORB特征匹配，然后执行RANSAC PnP以计算SE（3）变换的初始猜测。 然后，我们通过最小化3D和2D几何约束，使用Gauss-Newton方法优化Sim（3）变换。

令为在闭环匹配点中的重建特征，为他们的逆深度，是当前关键帧中的匹配功能，D是当前关键帧的稀疏逆深度图，通过将当前窗口中的活动地图点投影到当前关键帧中来计算。对于某些功能，我们可以在D中找到他们的深度，对于其他点，我们只有他们的2D像素位置，令指代没有深度的点，是那些有深度的，然后从闭环匹配点（参考）到当前关键帧的Sim（3）变换，可以通过最小化以下成本函数来估计：

是前面定义的投影和反投影函数是用于平衡不同测量单位的权重，在实践中，仅可以通过3D部件来估计比例，但是如果没有2D重投影误差，则如果估计的深度值是有噪声的，则旋转和平移估计将是不准确的。

D.滑动窗口与Sim（3）位姿图

在本节中，我们将解释如何融合滑动窗口的估计和全局位姿图。令与Xp为畸变坐标参数化后的当前窗口中关键帧的位姿，Xd为由反向深度d参数化的点，那么使用Levenberg-Marquardt（L-M）迭代的窗口优化问题是：

其中H是在L-M次迭代中近似为的Hessian矩阵， 是最佳增量，W是权重矩阵，，J为Jacobian矩阵，r为残差，它也可以写成块矩阵的形式：

众所周知，H具有类似箭头的稀疏模式（在DSO的公式中Hdd是对角矩阵），我们可以利用右下角的稀疏性来执行稀疏束调整[22]，[23]。

DSO中的边缘化策略将稀疏模式保持在Hdd中并且还保持先前在x上表示为二次函数的运动（详情请参考[1]中的方程（19））。 该先验也可以被视为姿势图中的超边缘，其约束内部的所有关键帧。 但是，传统的姿势图优化只需要在两个关键帧（如Tij）之间进行成对观察，并计算测量误差：

上冒号代表估计值。

由于我们的循环闭合方法计算循环候选和当前帧之间的相对姿势约束，我们还使用成对相对姿势观察来近似边缘化窗口内的约束。 具体来说，我们根据当前前端的全局姿态估计值计算这些观测值。

同样重要的是要注意，因为我们不想干扰局部窗口优化（它包含绝对姿势信息），在姿势图优化中我们将修复当前帧的姿势估计。 因此，姿势图优化将倾向于修改旧部分轨迹的全局姿势。 此外，在姿势图优化之后，不更新当前窗口中关键帧的全局姿势，以进一步确保局部窗口束调整不受全局优化的影响。 我们的实现基于g2o，这是[25]中提出的图优化库。

Ⅳ.评估—

我们在三个流行的公共数据集上评估我们的方法：TUM-Mono [24]，EuRoC MAV [26]和KITTI Odometry [27]，所有这些都在单目数据集中实现：

A.TUM-Mono数据集

TUM-Mono数据集是一个单目数据集，由50个室内和室外序列组成。 它提供了光度计相机校准，但没有完整的地面实况相机轨迹。 摄像机始终返回所有序列的起始点，使该数据集非常适合评估VO系统的累积漂移。 因此，我们在此数据集上禁用闭环检测功能，首先使用修改后的点选择策略方法评估VO精度。

我们评估了三种不同的点选择策略：（1）随机点选择; （2）DSO的原始方法和（3）我们的方法。 对于每个策略，我们在每个序列上向前运行10次，向后运行10次以考虑非确定性行为。 我们使用[24]中描述的方法计算关键帧轨迹中的累积平移，旋转和尺度漂移等。

图4显示了所有序列中的颜色编码对准误差。 图5显示了累积误差图，其描绘了其误差低于相应x值的运行次数（因此更靠近左上方更好）。 在这两个图中，我们看到将特征角点集成到DSO中并不会降低原始系统的VO精度。 另一个有趣的观点是，虽然随机选择会使跟踪更频繁地失败，但它似乎并没有增加成功跟踪序列的误差，如s01到s10。

为了显示一些定性结果，我们还运行LDSO，在TUM-Mono上闭环并得到一些Sim（3）闭合轨迹，如图6所示。重建地图的一个例子如图7所示。

B.EuRoc MAV数据集

EuRoC MAV数据集提供11个序列，包括立体图像，同步IMU读数和地面实况摄像机轨迹。 我们将LDSO与DSO和ORB进行比较，通过使用单目设定评估它们的均方根误差（RMSE）。 对于每个序列，我们在每个序列上运行10次，向后运行10次，结果显示在图8和图9中。一般来说，ORB-SLAM2在此数据集上运行良好，并且只能始终如一地运行 向前运行时序列V2-03。 DSO和LDSO都在序列V2-03上失败，但在大多数其他序列上，LDSO显着提高了摄像机跟踪精度。 循环闭合后的整体改进也可以在图9中找到。从图中我们可以看出ORBSLAM2更准确，而LDSO在这个数据集上更加鲁棒。

C.KITTI里程计数据集

在KITTI里程计数据集中，如[19]中的全面评估所示，单目VO系统如DSO和ORB-SLAM（仅限VO组件）遭受严重的累积漂移误差，这使得它们不能用于如此大规模的情况。 虽然解决这个问题的自然方法是集成IMU之类的其他传感器或者使用立体相机已经证明非常成功[19]，[18]，这里我们希望看到我们的单眼方法在整合闭环检测功能之后的潜力。 

D.运行时评估

最后，我们提出了关于点选择步骤的简短运行时间分析。 请注意，闭环仅偶尔发生，并且姿势图在单个线程中运行，因此它们不会影响主线程的计算时间。 我们在主线程中更改的是添加额外的特征提取和描述子计算步骤。 但与基于特征的方法不同，它们不是针对每个帧执行，而是仅针对关键帧执行。 表II显示了使用不同拣选策略的点选择步骤的平均计算时间。 由于特征和描述符提取，LDSO中的点选择比DSO中的点时间略多。值得注意的是，值是在关键帧上计算的，因此在对所有帧进行平均时，将进一步调节运行时影响。该程序在配备Ubuntu 18.04和Intel i7-4770HQ CPU和16GB RAM的笔记本电脑上进行测试。

Ⅴ.总结—

在本文中，我们提出了一种将闭环检测和全局映射优化集成到直接VO系统DSO系统中的方法。 DSO的原始点选择适用于包含可重复的功能。对于那些我们计算ORB描述符并构建用于循环闭包检测的BoW模型的人。我们证明了点选择保留了测距前端的原始鲁棒性和准确性，同时使后端能够有效地减少旋转，平移和缩放的全局漂移。我们相信所提议的方法可以扩展到VO或SLAM的未来改进。 例如，光度束调整层可能会提高全局地图精度。 为了确保长期操作，可以采用诸如关键帧剔除和去除冗余3D点的地图维护策略。 在闭环之后组合来自邻近关键帧的3D点的信息可以帮助进一步提高重建几何的精度。