NTU开源嵌入式平台AirSLAM：高效&鲁棒的点线视觉SLAM系统

作者 | Kuan Xu  编辑 | 3D视觉之心

点击下方卡片，关注“自动驾驶之心”公众号

戳我-> 领取自动驾驶近15个方向学习路线

>>点击进入→自动驾驶之心『SLAM』技术交流群

本文只做学术分享，如有侵权，联系删文

vslam的光照挑战

现有的vSLAM系统在面对严苛光照条件时仍然面临两类挑战：

1. 短期光照挑战：剧烈的光照变化或低光照环境下，特征检测和跟踪经常失败，从而严重影响轨迹估计的质量。它影响了两个时间上相邻帧之间的姿态估计

2. 长期光照挑战：当视觉地图用于重定位时，光照的变化可能显著降低成功率。它影响了查询帧与已有地图之间的匹配。

现有方法通常只聚焦于上述挑战中的某一个。例如，各种图像增强和图像归一化算法已经被开发出来，以确保鲁棒的跟踪。这些方法主要集中于保持全局或局部亮度的一致性，但它们通常难以处理所有类型的光照挑战。一些系统通过在包含各种光照条件的大型数据集上训练VO或SLAM网络来解决这一问题。然而，它们很难生成适合长期定位的地图。一些方法可以提供光照鲁棒的重定位，但通常需要在良好光照条件下构建地图。

本文介绍的AirSLAM[1]将点和线特征结合起来用于跟踪、建图、优化和重定位。为了在效率和性能之间取得平衡，设计了一个混合系统，采用基于学习的方法进行特征检测和匹配，采用传统的几何方法进行姿态和地图优化。此外，为了提高特征检测的效率，我们开发了一个统一的模型，能够同时检测点和线特征。我们还通过提出一个多阶段的重定位策略来应对长期定位的挑战，该策略能够有效地重用我们的点线地图。

C++源代码链接：https://github.com/sair-lab/AirSLAM

相比前作AirVO[2]，AirSLAM是扩展版本，其实质性的改进包括：

• 设计了一个统一的CNN来检测点和线特征，增强了特征检测在光照挑战环境中的稳定性。此外，我们使用了更高效的LightGlue进行特征匹配。

• 扩展了系统，使其支持立体数据和立体惯性数据，在提供惯性测量单元（IMU）的情况下提高了系统的可靠性。

• 集成了回环检测和地图优化，形成了一个完整的vSLAM系统。

• 设计了一个基于点和线特征的多阶段重定位模块，使得系统能够有效应对长期光照挑战。

系统概述

一个实用的vSLAM系统应具备以下特性：

• 高效性：系统应在资源受限的平台上具备实时性能。

• 可扩展性：系统应易于扩展，以适应各种需求和实际应用。

• 易于部署：系统应易于在真实机器人上部署，并能够实现稳健的定位。

该系统是一个混合系统，如图1所示，因为需要数据驱动方法的鲁棒性和几何方法的精度。它包括三个主要组件：立体视觉里程计（Stereo VO/VIO）、离线地图优化和轻量级重定位。

1. 立体视觉里程计（Stereo VO/VIO）：提出了一种基于点线的视觉里程计，能够处理立体视觉数据和立体惯性数据。

2. 离线地图优化：实现了几个常用的插件，如回环检测、位姿图优化和全局捆绑调整。系统可以通过添加自定义插件，轻松扩展以实现其他地图处理目的。例如，我们实现了一个插件，用于使用线特征的端点训练场景依赖的交汇点词汇，并在轻量级多阶段重定位中使用。

3. 轻量级重定位：提出了一种多阶段的重定位方法，提高了效率的同时保持了效果。在第一阶段，使用提出的PLNet检测关键点和线特征，并使用在大型数据集上训练的关键点词汇检索若干候选。在第二阶段，使用场景依赖的交汇点词汇和结构图快速过滤掉大部分错误的候选。在第三阶段，在查询帧和剩余候选之间进行特征匹配，以找到最佳匹配并估计查询帧的位姿。由于第三阶段的特征匹配通常是耗时的，因此第二阶段的过滤过程提高了系统的效率，相较于其他两阶段的重定位系统。

将一些耗时的过程，如回环检测、位姿图优化和全局捆绑调整，转移到离线阶段。这提高了在线建图模块的效率。在许多实际应用中，例如仓库机器人，通常由一个机器人构建地图，然后由其他机器人重用。设计的系统考虑到了这些应用需求。轻量级的映射和地图重用模块可以轻松部署在资源受限的机器人上，而离线优化模块可以在更强大的计算机上运行，用于各种地图操作，如地图编辑和可视化。映射机器人将初始地图上传到计算机，然后计算机将优化后的地图分发给其他机器人，确保无漂移的重定位。

特征检测

动机

基于学习的特征检测方法在光照挑战环境中展现出了比传统方法更稳定的性能。然而，现有的基于点线的视觉里程计（VO/VIO）和SLAM系统通常分别检测关键点和线特征。对于手工设计的方法，由于其效率较高，可以接受这种做法，但对于基于学习的网络来说，同时应用关键点检测和线检测网络，尤其是在立体配置中，会影响资源受限平台上的实时性能。因此需要设计一个能够同时检测关键点和线特征的高效统一模型。

然而，实现一个同时检测关键点和线特征的统一模型是有挑战性的，因为这些任务通常需要不同的真实图像数据集和训练过程。关键点检测模型通常在包含多种图像的大型数据集上训练，并依赖于提升步骤或图像对的对应关系进行训练。对于线特征检测，我们发现线框解析方法比自监督模型提供了更强的几何线索，因为它们能够检测到更长且更完整的线条。

架构设计

如图2所示，当可视化关键点和线特征检测网络的结果时，有两个发现：

1. 大多数由线特征检测模型检测到的线端点（交点）也被关键点检测模型选为关键点

2. 关键点检测模型输出的特征图包含了边缘信息。

因此，可以基于预训练的关键点检测模型的骨干网络构建一个线特征检测模型。基于这一假设，设计了PLNet以在一个统一框架中检测关键点和线特征。如图3所示，包括共享的骨干网络、关键点模块和线特征模块。

• 骨干网络：采用了SuperPoint的设计作为骨干网络，因为它具有良好的效率和效果。它使用了8层卷积层和3层最大池化层。输入是尺寸为𝐻 × 𝑊的灰度图像，输出为𝐻 × 𝑊 × 64、𝐻/2 × 𝑊/2 × 64、𝐻/4 × 𝑊/4 × 128、𝐻/8 × 𝑊/8 × 128的特征图。

• 关键点模块：同样遵循SuperPoint的设计来构建关键点检测头。它有两个分支：得分分支和描述符分支。输入是骨干网络输出的𝐻/8 × 𝑊/8 × 128特征图。得分分支输出尺寸为𝐻/8 × 𝑊/8 × 65的张量，其中65个通道对应一个8×8的网格区域和一个表示没有关键点的垃圾箱。该张量经过softmax处理后再调整尺寸为𝐻 × 𝑊。描述符分支输出尺寸为𝐻/8 × 𝑊/8 × 256的张量，用于插值计算关键点的描述符。

• 线特征模块：该模块以𝐻/4 × 𝑊/4 × 128的特征图为输入。它由一个类U-Net的CNN和线特征检测头组成。将U-Net进行了修改，使其包含更少的卷积层，从而提高了效率。类U-Net的CNN用于增加感受野，因为检测线条比检测关键点需要更大的感受野。使用EPD LOIAlign处理线特征模块的输出，并最终输出交点和线条。

网络训练

PLNet训练分为两轮。

1. 在第一轮中，仅训练骨干网络和关键点检测模块，这意味着我们需要训练一个关键点检测网络。

2. 在第二轮中，固定骨干网络和关键点检测模块，仅在Wireframe数据集上训练线特征检测模块。我们跳过了第一轮的详细内容，因为它们与非常相似，而是介绍线特征检测模块的训练过程。

• 线编码：采用吸引区域场来编码线段。对于线段l = (x1, x2)，其中x1和x2是l的两个端点，p是l的吸引区域中的一个点，使用p和四个参数来编码l：

其中𝑑是p到l的垂线段，𝜃是l与图像Y轴的夹角，𝜃1是p到x1的夹角，𝜃2是p到x2的夹角。

• 线特征预测：线特征模块输出一个尺寸为𝐻/4 × 𝑊/4 × 4的张量来预测参数，并输出一个热图来预测交点。对于每个通过参数解码的线段，从热图中选择与端点最接近的两个交点，并形成线条提议。然后使用EPD LOIAlign和一个头分类器来决定线条提议是否为真正的线特征。

• 线特征模块训练：使用𝐿1损失来监督参数的预测，并使用二元交叉熵损失来监督交点热图和头分类器的输出。总损失为它们的和。为了提高线特征检测在光照挑战环境中的鲁棒性，对训练图像进行了七种类型的光度数据增强处理。训练使用ADAM优化器，前35个epoch学习率为4𝑒-4，最后5个epoch学习率为4𝑒-5。

立体视觉里程计

概述

基于点线的立体视觉里程计（Stereo Visual Odometry）如图5所示。它是一个混合VO系统，结合了基于学习的前端和传统优化的后端。对于每对立体图像，首先使用所提出的PLNet提取关键点和线特征。然后使用一个图神经网络（LightGlue）来匹配关键点。同时通过关键点匹配结果关联线特征，并进行线特征匹配。接下来执行初始位姿估计并剔除离群值。基于这些结果，对关键帧的2D特征进行三角测量，并将其插入到地图中。最后，执行局部BA以优化点、线和关键帧的位姿。与此同时，如果可以获取惯性测量单元（IMU）的数据，会使用IMU预积分方法处理这些测量数据，并将其添加到初始位姿估计和局部捆绑调整中。

将基于学习的特征检测和匹配方法应用于立体VO是耗时的。因此，为了提高效率，在系统中使用了以下三种技术：

1. 对于关键帧，在左右图像上提取特征并执行立体匹配以估计真实尺度。但对于非关键帧，仅处理左图像。此外使用了一些宽松的标准，使得系统中选取的关键帧非常稀疏，因此系统中特征检测和匹配的运行时间和资源消耗接近于单目系统。

2. 将CNN和GNN的推理代码从Python转换为C++，并使用ONNX和NVIDIA TensorRT进行部署，其中16位浮点运算替代了32位浮点运算。

3. 设计了一个多线程管道。采用生产者-消费者模型，将系统分为前端线程和后端线程两大部分。前端线程负责特征提取和匹配，而后端线程执行初始位姿估计、关键帧插入和局部捆绑调整。

特征匹配

使用LightGlue来匹配关键点。对于线特征，目前大多数VO和SLAM系统使用LBD算法或跟踪采样点来匹配它们。然而，LBD算法从线的局部带状区域提取描述符，因此在光照变化或视点变化的情况下，线特征检测不稳定。跟踪采样点可以匹配在两个帧中检测到不同长度的线，但目前SLAM系统通常使用光流跟踪采样点，这在光照条件快速或剧烈变化时表现较差。一些基于学习的线特征描述符也被提出，但由于时间复杂度的增加，它们很少在现有SLAM系统中使用。

因此，为了解决效率和效果问题，我们设计了一种快速且鲁棒的线特征匹配方法，以应对光照挑战条件。首先，我们通过距离将关键点与线段关联。假设在图像上检测到𝑀个关键点和𝑁个线段，每个关键点表示为 ，每个线段表示为，其中为线段的参数，而()为端点。首先通过以下公式计算与之间的距离：

如果 且在坐标轴上的投影在线段端点的投影范围内，即或，将认为属于。然后，可以基于这两张图像的点匹配结果匹配线段。对于图像𝑘上的线段和图像上的线段，计算一个得分𝑆𝑚𝑛来表示它们是同一线段的置信度：

其中是属于的点特征和属于的点特征之间的匹配数量。和分别是属于和的点特征数量。然后，如果且，我们将认为和是同一线段。这种耦合特征匹配方法使线段匹配能够共享关键点匹配的鲁棒性能，同时由于不需要另一个线段匹配网络，它具有很高的效率。

3D 特征处理

1. 3D线段表示：我们使用Plücker坐标【71】来表示三维空间中的线段：

其中v是线段的方向向量，n是由线段和平面确定的法向量。Plücker坐标用于3D线段的三角测量、变换和投影。它是过参数化的，因为它是一个6维向量，而一个3D线段只有四个自由度。在图优化阶段，额外的自由度将增加计算成本并导致系统的数值不稳定。因此，还使用正交表示来表示3D线段：

Plücker坐标和正交表示之间的关系类似于SO(3)和so(3)。通过以下公式可以从Plücker坐标转换为正交表示：

其中Σ3×2是一个对角矩阵，其两个非零项可以通过SO(2)矩阵表示：

在实际操作中，可以通过QR分解简单快速地进行这种转换。

1. 三角测量：三角测量是从两个或更多2D线特征初始化一个3D线段。使用两种方法对3D线段进行三角测量。第一种方法3D线段的姿态可以通过两个平面计算。为实现这一点，选择两条在两张图像上的线段l1和l2，它们是一个3D线段的两个观测。注意，这两张图像可以来自同一个关键帧的立体图像对，也可以来自两个不同的关键帧。l1和l2可以反投影并构建两个3D平面π1和π2。然后，3D线段可以视为π1和π2的交线。

然而，三角测量一个3D线段比三角测量一个3D点更困难，因为它更容易受到退化运动的影响。因此，如果上述方法失败，还采用第二种线段三角测量方法，利用点来计算3D线段。为了初始化3D线段，选择两个已三角测量的点X1和X2，它们属于该线段并且在图像平面上与该线段的距离最短。然后，可以通过以下公式获得该线段的Plücker坐标：

该方法只需很少的额外计算，因为所选择的3D点在点三角测量阶段已经被三角测量。它非常高效且鲁棒。

1. 重投影：重投影用于计算重投影误差。我们使用Plücker坐标来进行3D线段的变换和重投影。首先，我们将3D线段从世界坐标系转换到相机坐标系：

其中和分别是相机坐标系和世界坐标系中的3D线段的Plücker坐标。是从世界坐标系到相机坐标系的旋转矩阵，是平移向量。表示向量的反对称矩阵，是从世界坐标系到相机坐标系的3D线段变换矩阵。

然后，可以通过线段投影矩阵将3D线段投影到图像平面：

其中 是图像平面上重投影的2D线段。表示向量的前三行。

关键帧选择

系统中使用的基于学习的数据关联方法能够跟踪具有大基线的两帧。因此，不同于其他VO或SLAM系统中使用的逐帧跟踪策略，只将当前帧与最后一个关键帧匹配。我们认为这种策略可以减少累积的跟踪误差。

因此，关键帧选择对系统至关重要。一方面，希望关键帧稀疏以减少计算资源的消耗。另一方面，关键帧越稀疏，发生跟踪失败的可能性就越大。为了平衡效率和跟踪的鲁棒性，如果满足以下任一条件，则选择该帧为关键帧：

• 跟踪到的特征数少于。

• 当前帧与最后一个关键帧之间的跟踪特征的平均视差大于。

• 跟踪到的特征数少于。

其中𝛼1, 𝛼2和𝑁𝑘𝑓均为预设阈值。𝑁𝑠是检测到的特征数。𝑊和𝐻分别表示输入图像的宽度和高度。

局部图优化

为了提高精度，在插入新关键帧时执行局部捆绑调整。选择最近的个相邻关键帧构建一个局部图，其中地图点、3D线段和关键帧作为顶点，位姿约束作为边。我们使用点约束和线约束，以及如果有IMU数据可用，还会使用IMU约束。相关的误差项定义如下：

1. 点重投影误差：如果帧𝑖可以观察到3D地图点X𝑝，则重投影误差定义为：

其中, 是帧上的观测值，表示相机投影。

1. 线重投影误差：如果帧𝑖可以观察到3D线段，则重投影误差定义为：

1. IMU残差：我们首先按照【69】在帧𝑖和帧𝑗之间预积分IMU测量值：

IMU残差定义为：

成本函数定义为：

使用Levenberg-Marquardt优化器来最小化成本函数。如果点和线的残差过大，在优化过程中它们也会被拒绝为离群值。

初始地图

地图在离线阶段进行了优化。因此，当视觉里程计完成后，关键帧、地图点和3D线段将被保存到磁盘，以便后续优化使用。对于每个关键帧，保存其索引、位姿、关键点、关键点描述符、线特征和交点。2D特征与3D特征之间的对应关系也会被记录下来。为了加快地图的保存、加载和在不同设备之间的传输，以上信息以二进制形式存储，这也使得初始地图比原始数据要小得多。例如，在OIVIO数据集上，我们的初始地图大小仅约为原始数据大小的2%。

地图优化与重用

离线地图优化

这一部分旨在处理由我们的视觉里程计（VO）模块生成的初始地图，并输出可以用于无漂移重定位的优化地图。我们的离线地图优化模块包括以下几个地图处理插件。

1. 回环检测：与大多数当前的vSLAM系统类似，使用粗到细的流程来检测回环。回环检测依赖于DBoW2来检索候选帧，并使用LightGlue进行特征匹配。在包含35,000张图像的数据库上为PLNet检测到的关键点训练了一个词汇。这些图像从几个大型数据集中选取，包括室内和室外场景。该词汇有4层，每层10个节点，因此包含10,000个词。

2. 地图融合：回环对观察到的3D特征通常被错误地当作两个不同的特征。因此，目标是融合由回环对观察到的重复点和线特征。对于关键点特征，使用上述回环对之间的特征匹配结果。如果两个匹配的关键点分别关联了两个不同的地图点，将它们视为重复特征，并且只保留一个地图点。2D关键点与3D地图点之间的对应关系以及共视图图中的连接也将更新。

   对于线特征，首先通过2D-3D特征对应关系和2D点线关联来关联3D线段和地图点。然后，检测与同一地图点关联的3D线段对。如果两个3D线段共享超过3个关联的地图点，它们将被视为重复线段，并且只保留一个3D线段。

3. 全局BA：在融合重复特征后执行全局捆绑调整（GBA）。其残差和成本函数类似，不同之处在于该模块中所有关键帧和特征都将被优化。在优化的初始阶段，由于VO漂移误差，融合特征的重投影误差相对较大，因此我们首先迭代50次而不剔除离群值，以便将变量优化到一个较好的初始位置，然后再进行另外40次带有离群值剔除的迭代。

   我们发现当地图较大时，初始的50次迭代无法将变量优化到令人满意的位置。为了解决这一问题，如果地图包含超过80,000个地图点，将在全局BA之前首先执行位姿图优化（PGO）。在PGO中，仅调整关键帧的位姿，成本函数定义如下：

在线回环检测系统通常在检测到一个新的回环后执行全局捆绑调整，因此当一个场景中包含许多回环时，它们会经历多次重复的全局捆绑调整。相比之下，离线地图优化模块仅在检测到所有回环后才执行全局捆绑调整，这使得与这些系统相比，可以显著减少优化迭代次数。

1. 场景依赖词汇：训练了一个用于重定位的交点词汇。该词汇建立在地图中的关键帧交点之上，因此具有场景依赖性。与关键点词汇相比，用于训练交点词汇的数据库通常要小得多，因此我们将层数设置为3，每层10个节点。这个交点词汇非常小，只有大约1MB，因为它只包含1,000个词。

2. 优化地图：保存优化后的地图以供后续地图重用，保存了更多的信息，例如每个关键帧的词袋、全局共视图图以及场景依赖的交点词汇。同时，由于重复地图点和3D线段的融合，3D特征的数量有所减少。因此，优化后的地图占用的内存与初始地图相当。

地图重用

在大多数vSLAM系统中，识别已访问的地方通常需要两个步骤：

1. 检索𝑁𝑘𝑐个与查询帧相似的关键帧候选

2. 执行特征匹配并估计相对位姿。

第二步通常是耗时的，因此选择合适的𝑁𝑘𝑐非常重要。较大的𝑁𝑘𝑐会降低系统的效率，而较小的𝑁𝑘𝑐可能会使正确的候选无法被召回。例如，在ORB-SLAM3的回环关闭模块中，只使用DBoW2检索到的最相似的三个关键帧来提高效率。这在两帧之间时间间隔较短且光照条件相对相似的回环对中表现良好。然而，对于具有挑战性的任务，如日/夜重定位问题，检索如此少的候选通常会导致较低的召回率。然而，检索更多候选需要对每个查询帧进行更多次特征匹配和位姿估计，这使得在实时应用中很难实现。

为了解决这个问题，提出了一种高效的多阶段重定位方法，使优化地图可以在不同的光照条件下使用。我们的见解是，如果能够快速过滤掉大多数错误的候选，那么在保持或甚至提高重定位召回率的同时，也可以提高效率。因此，在上述两步流程中添加了另一个步骤。接下来，将详细介绍所提出的多阶段流程。

1. 第一步：这一步是从地图中检索与查询帧相似的关键帧。对于每个输入的单目图像，使用PLNet检测关键点、交点和线特征。然后，执行一个“粗略候选选择”的流程，但有两个不同之处。第一个不同点是不使用共视图图过滤候选，因为查询帧不在图中。第二个不同点是保留所有候选用于下一步，而不仅仅是三个候选。

2. 第二步：这一步使用交点和线特征过滤掉大多数在第一步中选定的候选。对于查询帧K𝑞和每个候选帧K𝑏，首先通过交点词汇过滤掉大多数候选。在前面的步骤中，已经检测了K𝑞的交点。接下来，使用DBoW2词袋将这些交点量化为单词，并通过计算直方图的𝜒2距离来选择与K𝑞相似的候选帧。如果𝜒2距离大于0.3 · 𝑆𝑚𝑎𝑥，K𝑏将被过滤掉。在剩余的候选中，如果K𝑞和K𝑏之间共享超过4条线特征，并且这些线特征的端点足够接近，它们将保留。否则，K𝑏将被过滤掉。这一步使得通过接下来耗时的特征匹配和位姿估计进行处理的候选数量大大减少。

3. 第三步：这一步对第二步中剩余的候选执行特征匹配并估计位姿。首先通过LightGlue将K𝑞和K𝑏的关键点和交点进行匹配。然后，基于上述点匹配结果匹配线特征。最后，通过五点法估计初始相对位姿，并通过迭代最近点算法（ICP）优化它。

通过这种多阶段流程，我们能够提高重定位的召回率和精度，同时保持实时性能。

实验效果

总结一下

AirSLAM系统是一种高效且光照鲁棒的点线视觉SLAM系统。设计了一个统一的CNN模型来同时检测点和线特征，并提出了一种基于点线特征的多阶段重定位方法。这些创新使得系统能够在短期和长期光照挑战中表现出色，同时保持足够的效率以部署在嵌入式平台上。最终将AirSLAM部署在实际机器人中，显示出其在各种真实场景下的广泛适用性。

参考

[1] AirSLAM: An Efficient and Illumination-Robust Point-Line Visual SLAM System

[2] Airvo: An illumination- robust point-line visual odometry

投稿作者为『自动驾驶之心知识星球』特邀嘉宾，欢迎加入交流！重磅，自动驾驶之心科研论文辅导来啦，申博、CCF系列、SCI、EI、毕业论文、比赛辅导等多个方向，欢迎联系我们！

① 全网独家视频课程

BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

网页端官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

国内最大最专业，近3000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦感知、定位、融合、规控、标定、端到端、仿真、产品经理、自动驾驶开发、自动标注与数据闭环多个方向，目前近60+技术交流群，欢迎加入！扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

④【自动驾驶之心】全平台矩阵