基于多摄像头的高鲁棒性视觉SLAM系统设计

爆改ORB-SLAM2，能否实现更高鲁棒性的多摄像头SLAM系统？

作者：

Han Song， Cong Liu， Huafeng Dai

01   摘要

多摄像头SLAM系统提供了许多优势，主要源于其能够整合更广泛视野的信息，从而提高鲁棒性和定位精度。在这项研究中，我们对现有的立体SLAM系统ORB-SLAM2进行了重大扩展和改进，旨在实现更高的精度。为了实现这一目标，我们首先将所有摄像头的测量映射到一个名为BundledFrame的虚拟相机上。这个虚拟相机经过精心设计，可以无缝适应多摄像头配置，有助于有效地融合来自多个摄像头的数据。此外利用捆绑调整（BA）过程中的外参，实现精确的轨迹估计。此外对多摄像头场景中捆绑调整（BA）的作用进行了广泛的分析，探讨了其对跟踪、局部建图和全局优化的影响。我们的实验评估涉及对比地面真实数据和最先进的SLAM系统。为了严格评估系统的性能，使用了EuRoC数据集。我们评估的结果一致表明，与现有方法相比，该系统具有更高的准确性。

02   主要贡献

本文的主要目标是通过扩展ORB-SLAM2的功能来增强准确性，从多个摄像头中的姿态估计和地图重用开始。所有这些多摄像头的图像特征将被合并到跟踪模块中进行特征匹配，以及在闭环检测期间进行位置识别。此外通过最小化涉及多个摄像头的成本函数来实现姿态更新和优化。在将视觉SLAM系统视为虚拟传感器方面，我们受到了Wang等人的工作的启发。我们首先将所有摄像头的测量映射到一个称为BundledFrame的虚拟相机上。这个虚拟相机被设计成可以无缝适应多摄像头设置，使我们能够高效地合并来自多个摄像头的数据，然后使用外部参数进行捆绑调整，以优化多摄像头SLAM系统中的姿态。我们的多摄像头系统的主要贡献可以总结如下： 

全面性：我们为多摄像头提供了一个完整的SLAM系统，包括闭环检测和地图重用。 

可扩展性：通过利用一种称为“Bundled”的高效数据结构，我们将多个摄像头的数据合并到一个“BundledFrame”或“BundledKeyframe”中。这形成了所有系统操作的基础，包括跟踪、位置识别和优化。我们的系统通过在摄像头之间实现外部参数的捆绑调整，可以轻松适应额外的摄像头。

03   内容概述

A. 系统概述 

图1:BundledSLAM的方案流程

图1展示了我们多摄像头SLAM系统的流程，我们的系统分为三个主要的并行线程：跟踪、局部建图和闭环检测。跟踪模块负责通过在局部BundledMap中识别特征匹配来估计增量运动，并使用我们的仅运动Bundle调整（BA）来最小化重投影误差。它还确定当前帧是否符合新的BundledKeyframe，随后将其整合到局部建图线程中。局部建图线程管理新的BundledKeyframes，涉及一致性连接更新、新地图点的创建和冗余数据的删除。通过实现我们的局部Bundle调整（BA），它优化了局部BundledMap。闭环检测线程的主要目标是检测重要的环，并执行位姿图优化。此外，该线程启动另一个线程来进行全局Bundle调整（BA），以纠正累积的误差。与先前基于特征的SLAM系统类似，BundledSLAM首先对输入图像进行预处理，以在显著的关键点位置提取特征。一旦特征提取完成，原始输入图像将被丢弃，所有后续的系统操作都依赖于这些特征，用于跟踪、位置识别和优化等任务。

图2：Bundled的示例，包括唯一特征ID、单眼点和匹配点。唯一特征数 = 单眼特征数 + 匹配特征数

为了促进来自不同摄像头的图像特征的集成，我们实现了跨多个摄像头的特征匹配，为每个特征点分配一个唯一的特征ID。这些唯一特征ID与它们对应的摄像头ID（每个观察到的摄像头）之间的关联存储在我们的专用数据结构'Bundled'中，如图2所示。只由单个摄像头观察到的特征点称为“单眼特征点”，而其他特征点则称为“匹配点”。Bundled提供了比ORB-SLAM2更广泛的特征搜索范围，后者仅考虑左-左摄像头之间的匹配，以找到准确的图像特征和地图点。

B.BundledFrame、BundledKeyframe和BundledMap

图3：BundledFrame 的总体概览

BundledFrame（见图3）包含来自不同摄像头的所有图像信息。此外，来自不同摄像头的帧分别划分为 64×48 大小的网格，并根据位置将特征分配到相应的网格中，以降低匹配的时间复杂度。BundledKeyframe 在跟踪的最后一步决定当前 BundledFrame 是否被选为新的 BundledKeyframe。同时，BundledKeyframe 是本地映射和本地闭合的基本操作单元。共视性表示为无向加权图。如果两个 BundledKeyframes 共享相同地图点的观测，则存在一个共视边。接下来的本地 BA 和闭环依赖于这些共视信息。BundledMap 由所有 BundledKeyframes 和被 BundledKeyframes 观察到的所有地图点的集合组成。

C.多相机 SLAM 系统中相机投影模型

相机状态向量: 在时间步 k，第 i 个摄像头的状态向量包括相对于世界坐标系的传感器方向和位置，表示为 cki = [Rkiw, tk_iw; 0, 1]。其中，Rkiw 是旋转矩阵，表示世界坐标系到第 i 个摄像头坐标系的转换，而 tk_iw 是位置向量，表示相机在世界坐标系中的位置。

相机投影模型: 使用与已有研究相同的校准相机投影模型，描述特征点 j 的观测。这个投影模型将特征点的三维位置向量 Pj 投影到第 i 个摄像头的图像平面上，并考虑了测量噪声。投影后的像素位置表示为 zji，其中 h(·) 是相机的投影函数，nji 是测量噪声。

姿态更新: 对于特定的时间步 k，通过测量来自所有摄像头的数据，计算第一个摄像头 Ck1 的姿态更新。然后，通过相对于第一个摄像头的估计恒定转换来更新其他摄像头的姿态。这个相对转换 Ti1 是固定的，因为它是不可观测的，避免了整个系统由于摄像头和转换的自由运动而导致的漂移。

D.多相机的运动估计和束调整

我们的目标是准确地估计每个时间步的相机姿态，以便构建出准确的地图。首先介绍了我们的运动估计方法，为每个时间步的第一个相机姿态赋予初始值，如果上一帧中的跟踪成功，我们就将初始值设为前一相对运动，假设了一个恒定速度运动模型。然后，我们对相机姿态进行更新和优化，这个过程分为三个部分：只考虑运动的束调整（Motion-only Bundle Adjustment，简称 Motion-only BA）、局部束调整（Local BA）和全局束调整（Global BA）。

在 Motion-only BA 中，我们的目标是找到使得在当前时间步的相机姿态估计与地图点的投影误差最小化的最佳相机姿态。我们定义了一个成本函数，将所有相机在当前时间步观察到的地图点与其对应的特征点之间的重投影误差加权求和，并利用迭代方法来最小化这个非线性成本函数。接着，我们介绍了我们系统中多相机的投影模型。我们使用了单目投影模型，而不是 ORB-SLAM2 中的矫正立体投影，以适应多相机系统。我们给出了单目投影的数学表示，并说明了它是如何用于姿态更新的。

在 Local BA 中优化了一组共视的束关键帧和所有这些关键帧中观察到的地图点。为了防止变量收敛到零空间，我们使用了与 ORB-SLAM2 相同的策略。对于不属于当前局部束调整的关键帧，但是观察到了局部地图点的其他关键帧，则会在优化过程中保持不变。最后介绍了全局 BA，它对所有的束关键帧和地图点进行了优化，除了原点束关键帧。与局部 BA 类似，全局 BA 也使用了相同的优化策略来最小化成本函数，以进一步提高系统的精度和鲁棒性。

图4：具有多个同步摄像头的视觉里程计,目标是估计BundledFrame或BundledKeyframe中第一个相机Ck1在世界坐标的每一时刻的相对运动。

E.回环闭合

回环闭合包含三个关键步骤：回环检测、相似性变换计算和回环融合。在 BundledSLAM 中，回环检测依赖于数据库查询，采用基于 DBoW2 的词袋模型地点识别模块。利用从大量图像数据集中提取的 ORB 描述符创建了一个视觉词汇，以确保在具有相同词汇的不同环境中获得鲁棒的性能。我们系统中的每个唯一特征描述子都被分配给词汇表中的特定视觉词。与 ORB-SLAM2 不同，BundledSLAM 根据 BundledKeyframe 信息增量构建数据库，包括一个反向索引。该索引跟踪了每个视觉词汇在词汇表中被哪些 BundledKeyframes 观察到。因此BundledKeyframe 数据库为回环候选项提供了更广泛的搜索范围，比 ORB-SLAM2 实现的回环闭合更准确。此外在查询识别数据库时，计算当前 BundledKeyframe Bi 和其在共视图中的所有邻居的词袋向量之间的相似性，并设置一个阈值得分 smin。BundledSLAM 在回环候选项检测方面比 ORB-SLAM2 更为严格，因为它丢弃了所有得分低于 smin 的 BundledKeyframes，这种严格的方法提高了系统中回环闭合的精度。

04   实验

BundledSLAM 系统在配备有 Intel Core i7-8700（四核 @ 3.2GHz）处理器和16GB内存的 PC 上运行。为了评估我们系统的性能，使用了 EuRoc 数据集进行了评估，将系统的性能与当前最先进的 SLAM 系统，即 ORB-SLAM2 和 VINS-Stereo 进行了比较，以突出我们系统的准确性。考虑到多线程系统的非确定性以及所涉及的固有随机性，对每个序列执行了五次。这种方法不仅可以展示最佳结果，还可以提供中位数结果，全面展示了在估计轨迹方面实现的准确性。

A. EuRoC数据集 

我们对IMU-PARSAC在手工场景和公共数据集ADVIO上进行了定性和定量评估。我们的手工场景包括静态背景和前景的移动物体，有些物体偶尔会遮挡视野的大部分区域，以测试IMU-PARSAC的能力。图7比较了两种不同场景下几种异常值移除方案的效果，包括传统的鲁棒估计器RANSAC、动态对象分布先验估计器PARSAC以及我们提出的IMU-PARSAC。我们还比较了使用IMU预积分预测姿势来消除视觉观测中的异常值的能力。我们对当前帧中可见的2D点和地图中的3D点执行PnP几何估计，以确定观察到的2D点是否对应于移动物体。

B. 精度评估 

表I提供了BundledSLAM在所有序列中的绝对平移误差（均方根误差 - RMSE）的概述，与ORB-SLAM2和VINS-Stereo进行了比较。在将估计的轨迹与地面真实数据对齐后，计算了这些误差。值得注意的是，我们在处理序列MH_05_difficult、V1_02_medium、V1_03_difficult、V2_01_easy和V2_02_medium时，为我们的系统和ORB-SLAM2都激活了具有全局束调整（BA）的回环闭合模块。然而，在序列V2_03_difficult的某些部分由于高运动速度，两种方法都遇到了困难。通过加入额外的传感器，如惯性测量单元（IMU），这一挑战有可能得到缓解。作为一个多相机系统，BundledSLAM在精度上表现出比最先进系统ORB-SLAM2更好的优势。值得注意的是，BundledSLAM和ORB-SLAM2在所有序列中始终优于VINS-Stereo。每个序列的表现最佳方法已用粗体突出显示。 

图5：与ORB-SLAM2和VINS Stereo相比，EuRoC数据集中的轨迹。

图5比较了ORB-SLAM2、VINS-Stereo和地面真实数据。显而易见的是，我们的多相机系统在EuRoC数据集上测试时始终提供更准确的估计。此外，我们还对ORB-SLAM2、VINS-Stereo和我们的BundledSLAM系统之间的绝对姿态误差（APE）进行了比较，结果见图6。这些结果清楚地表明，我们提出的系统BundledSLAM始终优于其他系统，在每个序列中都表现出最小的APE。

图6：EuRoC数据集中绝对姿态误差（APE）的比较。

05   总结

本文介绍的BundledSLAM是一个旨在利用多相机功能的视觉SLAM系统。该系统将来自各种相机的数据集成到统一的“捆绑帧”结构中，从而实现了实时姿态跟踪、用于姿态和地图点优化的局部建图，以及闭环以确保全局一致性。我们使用EuRoC数据集进行的评估突显了我们的系统始终优于原始系统，展示了其在最佳结果和平均结果中的异常准确性。为了提高系统的鲁棒性，特别是在运动模糊或纹理特征有限的情况下，我们打算探索传感器融合，可能会引入惯性测量单元（IMU）。然而，我们意识到额外传感器可能引入的计算复杂性。作为我们未来研究的一部分，我们将优先考虑采用策略来减少这种复杂性，同时保持甚至提高系统性能。

审核编辑：黄飞