半直接单目VO | PL-SVO：快速、鲁棒的SLAM线特征跟踪

作者 | 猛女瘦瘦  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/571034195

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

ADAS巨卷干货，即可获取

后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！

《PL-SVO: Semi-Direct Monocular Visual Odometry by Combining Points and Line Segments》

摘要

摘要：大多数视觉里程计的方法都是基于点特征来估计相机的运动。因此，在低纹理的场景中，它们的性能会变差，因为很难找到一组可靠的特征。本文扩展了一种流行的半直接的单目视觉里程计，即SVO1，以处理线段，从而获取一个更强大的系统，能够处理纹理和结构化的环境。提出的里程计系统允许快速跟踪线段，因为它消除了在后续帧之间不断提取和匹配特征的必要性。当然，该方法比原来的SVO有更高的计算负担，但它仍然可以在个人电脑上以60Hz的频率运行，同时在更广泛的情况下表现得很稳健。

介绍

视觉里程计（VO）作为导航系统的一个重要组成部分，在无人驾驶飞行器（UAVs）或自动驾驶汽车等机器人应用中的重要性日益增加。VO问题的解决方案已经采用了不同的传感器，如单眼或立体相机[2] [3] [4]、RGB-D相机[5] [6]，或将其中任何一个与惯性测量单元（IMU）相结合[7]。传统的方法包括检测和匹配帧之间的点特征，然后，通过最小化观察点和投影点之间的重投影误差来估计摄像机的运动[8]。在这种情况下，这种方法在图1中描述的低纹理场景中的性能会恶化，因为在这种情况下很难找到大量或分布良好的图像特征。相比之下，线段通常在人类制造的场景中很丰富，其特点是具有丰富的边缘和线性形状的规则结构。处理图像中的线段并不像处理点那样简单，因为它们很难表示[9]，而且在检测和匹配任务中也需要很高的计算负担，因此只有少数解决方案被提出[10][11]，几乎没有达到实时规范。此外，基于边缘的算法也被用于解决跟踪问题[12] [13] [14]，以及估计摄像机的运动[15]。然而，这些方法需要一个相当昂贵的直接对准，这使得它们不太适合实时使用，也限制了它们对窄基线估计的应用。据我们所知，本文提出了第一个实时的单目视觉里程计（MVO）方法，它整合了点和线段的特征，因此它能够在结构化和纹理化的场景中稳健地工作。开发的C++ PL-SVO库的源代码和该建议的说明性视频可在这里找到：http://mapir.isa.uma.es 。

相关工作

视觉里程计算法可以分为两大类。第一类，被称为基于特征的算法，提取一组图像特征（传统意义上的点）并沿着连续的帧追踪它们。然后，它们通过最小化对应的观察到的特征和不同帧的投射特征之间的重投影误差来估计姿态。文献为我们提供了几种基于点的里程计问题的方法，如PTAM[16]，作者报告了一个快速的SLAM系统，能够对成千上万的landmarks进行实时的平行跟踪和映射。相比之下，用线状特征进行运动估计的问题由于其固有的困难而较少被探讨，特别是对单目里程计而言。在[17]中，作者将迭代最接近点（ICP）方法[18]扩展到带有线段的双目里程计中，他们用一对多的线段匹配方法替代了昂贵的描述符的计算。在我们以前的工作中[19]，我们提出了一个基于点和线段特征的双目视觉里程计系统。每个特征的影响都是用它们的协方差矩阵的倒数来加权的，这是由不确定性传播技术在重投影函数上得到的。然而，这项工作仍然依赖于传统的特征检测和匹配，因此它的计算成本很高。

另一组被称为直接方法，通过最小化几个图像位置的连续帧之间的光度误差来估计摄像机的运动。在[20]中，作者提出了一种直接的方法，称为DTAM，他们通过在每个关键帧之间直接对齐完整的强度图像，采用密集的深度图估计来估计摄像机的姿势。然而，这种方法需要GPU并行化，因为它要处理整个图像。为了处理直接方法的高计算要求，[21]中提出了一种新的单目技术，作者用半密集的方法估计摄像机的运动，从而在CPU上达到实时性能。他们对具有足够图像梯度的区域持续估计和跟踪半密集逆深度图，因此只利用那些为系统引入有效信息的区域。然后，他们通过最小化感兴趣区域的光度误差来估计摄像机的运动，因此将直接算法的良好特性与允许快速处理的稀疏方法相结合。

贡献

在低纹理和结构化的场景中，点的特征不太丰富，因此，基于点的视觉里程计算法的鲁棒性和准确性会急剧下降。另一方面，检测和匹配线段需要大量的计算资源，这也是缺乏使用这些特征的实时视觉里程计方法的主要原因。在这项工作中，我们将[1]中的半直接方法扩展到线段的情况，它作为稀疏直接运动估计的隐含结果，执行快速特征跟踪。因此，我们利用这种稀疏的结构来消除昂贵的线段检测（我们只在新的关键帧被引入框架时检测）和描述符计算，同时保持线段的良好特性。因此，由于结合了点和线段特征的信息，我们提供了一个快速的单目里程计系统，能够在低纹理的情况下稳健地工作。在下文中，我们描述了所提出的系统，并通过在不同环境中的实验验证了系统的可行性。

系统概述

所提出的系统可以被理解为[1]中半直接框架的扩展，它不仅考虑点，还考虑场景中的线段特征，并在管道中引入两者。这是一个不简单的扩展，因为从几何学的角度来看，线段比点的特征更复杂。在实践中，这使得某些对点来说几乎是微不足道的图像操作，在段的情况下变得更加繁琐的计算。因此，我们需要进行一些近似运算，并采取一些有根据的启发式方法，以节省计算资源。这些将在第三节和第四节中更详细地介绍。

为了完整起见，我们简要回顾一下半直接SVO的框架[1]的每一个阶段，如图2所示，同时说明这种半直接方法的合作和使用分段特征是如何变得互利的。半直接方法分为两个平行线程，一个用于估计摄像机的运动，另一个用于构建环境地图。

当前帧和上一帧之间的相对pose为新图像中的重投影点的位置提供了参数。我们进行（稀疏的）图像排列，以找到对应于同一三维点（蓝色方块）的图像patch之间光度差异最小的姿态。对于分段的点，在三维端点之间进行了均匀的采样。注意，在所有的图中，要优化的参数用红色画出，优化成本用蓝色强调。

在运动线程中，通过使用稀疏的直接对齐方法（见图3），在连续帧之间进行初始运动估计，利用已知三维特征提供的三维仿射，使patch之间的光度误差最小。这允许在帧之间快速跟踪特征，作为半直接运动估计的结果，这消除了执行帧与帧之间检测和匹配的需要。这对于点状特征来说是相当有利的，同时对于段状特征来说也是极为有利，因为它们的计算成本相当高。相反，只有在插入新的关键帧时才会检测特征，因此LSD片段检测的总体成本[22]变得可以承受。此外，通过将优化问题的维度减少到对姿势的估计，只自动满足了上极几何学的要求，我们不需要照顾到离群的匹配。

每个点的二维位置都是单独优化的，以最小化其patch的光度误差。对于线段来说，端点也同样被优化。这就减轻了地图和相机姿态估计中传播的误差。这个图也是改编自[1]。

然后，运动线程的第二步（见图4）是细化由直接对准的变换估计所给出的特征投影，从而避免违反极点约束以减少相机的漂移。完善特征的方法是将视角最接近的那一个作为参考patch。这种方法同样对片段非常有利，因为它在追踪片段的过程中限制了大的观测基线。因此，它缓解了众所周知的线段问题，如端点重复性、遮挡或因视角变化而导致的线段变形[11]。最后，摄像机的运动和地图的结构都是通过最小化重投影误差来完善的。

在最后一个运动估计步骤中，相机的pose和结构（三维点和片段）被优化，以最小化前一个特征对齐步骤中建立的重投影误差。与之前的类似，此图改编自[1]。

在这一点上，由于中间的连续跟踪，与远处特征的匹配已经完全解决，我们可以应用特定的基于特征的细化方法，这些方法对片段特征表现得相当好，如图5所描述。

总而言之，我们看到在半直接框架[1]中引入分段可以比其他更传统的方法做得更完美，因为初步的直接步骤缓解了历史上阻碍在视觉运动学中使用分段的大部分缺点。否则，运动和映射都会因使用分段而变得更加丰富，而不会给整个系统带来巨大的开销。

同时，地图线程用概率贝叶斯过滤器估计二维特征的深度，当一个关键帧被插入到地图时，该过滤器被初始化。深度过滤器的初始化具有很高的不确定性，但它们在几次迭代后会收敛到实际值，然后被插入到地图中，成为有用的运动估计。在下文中，我们将详细描述该算法的每个阶段，然后用真实环境中的实验对其进行验证。

半直接单目视觉里程计

基于稀疏模型的图像匹配

其中，指的是沿线段均匀定义的中间点。

然后，一个线段的强度残差被定义为同一三维线段点的像素之间的光度差，即:

其中，在n=0和，点分别指的时端点p和q。然后，我们估计最佳pose增量，使得对于所有点和线段特征的patch光度误差最小。

与SVO1类似，我们采用了[23]中提出的逆合成公式，以加快最小化过程。在这种情况下，我们寻求线段残差的线性化雅各布，它可以表示为每个中间点采样的单个点的雅各布的总和。

其表达式可以从SVO1[1]中得到。然后，我们通过对上述（3）中的成本函数进行稳健的高斯-牛顿最小化来估计最佳姿势。请注意，这个公式允许快速跟踪线段，如图1所示，由于传统的基于特征的方法所使用的高计算负担，这是一个开放的问题[19]。

独立的特征对齐

与SVO1[1]类似，我们通过最小化当前图像中的patch与该特征的所有三维观测值的投影之间的光度误差来单独细化每个特征的二维位置，这可以通过采用Lucas-Canade算法[23]来解决。在线段的情况下，我们只需要细化二维端点的位置（见图4），这决定了在估计投影误差时采用的线段方程：

在线段的情况下，这是一个大胆的假设，因为它们的端点比关键点的描述性要小得多。为了处理这个问题，我们也对（5）进行了稳健的优化，然后我们通过细化端点的三维位置来放松这个假设。请注意，在这一步有必要采用仿射变换，因为我们投射特征的最近的关键帧通常比较远，而且patch的大小也比上一步大。

姿势和结构的细化

在通过极线约束单独优化图像中每个特征的位置后，必须通过最小化三维特征与图像中相应的二维特征位置之间的重投影误差来进一步完善（见图5）。为此，我们考虑了在世界坐标系中的三维特征和摄像机姿势之间的重投影误差，因为它可以大大减少估计轨迹的漂移。采用这两种特征时的成本函数是：

其中表示得是点特征得重投影误差，表示得是线特征的重投影误差：

这是用高斯-牛顿（Gauss-Newton）迭代求解的，为此我们需要将（6）纳入一个稳健的框架，为此我们采用Cauchy损失函数:

优化包括三个步骤：i)我们首先用所有的样本来估计摄像机的运动；ii)我们过滤掉离群点，这被认为是那些残余误差超过这些误差的两倍的特征；iii)我们通过对离群子集的优化来快速地完善摄像机的姿势。最后，我们通过最小化重投影误差来完善三维点和线段特征的位置。

建图

地图线程递归地估计图像特征的三维位置，而这些特征的深度仍然是未知的。为此，SVO1[1]的作者在贝叶斯框架中实现了一个深度过滤器，为此他们用高斯+均匀混合模型分布对特征的深度进行建模[24]。在线段的情况下，我们需要估计端点的三维位置，因为它们被用于描述特征和估计重投影误差。然而，通过检测器（如LSD[22]）获得的线段的端点是不重复的，这是在单目视觉里程计中使用它们的一个限制。另一方面，这里采用的快速跟踪的优点之一是，我们在连续的帧中明确地寻找完全相同的线段，因此我们连续跟踪端点的位置。这样就可以在类似的贝叶斯框架中引入端点，在插入新的观测值时估计两个端点的分布。结果，我们得到了有意义的地图，可以用来提取关于场景几何的有用信息。

试验验证

在本节中，我们说明了在运动估计中包括线段的好处，特别是在低纹理环境中工作时。为此，我们从合成和真实的数据集中估计了几个序列中的单目相机的轨迹。所有的实验都是在Intel Core i5-6600 CPU @ 3.30GHz上进行的，没有GPU并行化。

在ICL-NUIM数据集中的评估

首先，我们在伦敦帝国学院和爱尔兰国立大学梅努斯分校（ICL-NUIM）的数据集中测试我们的算法[25]。该数据集由两个不同的合成环境组成，一个是办公室，另一个是客厅，可以为其生成和渲染几个序列（见图6）。表三比较了所提出的算法与SVO[1]在序列lrkt-2和ofkt-3上的性能。为了公平起见，我们采用了我们目前的PL-SVO的实现，没有在框架中引入线段作为比较的基线。结果显示，在第一个序列lrkt-2中，PL-SVO的性能优越，它能够沿着整个轨迹估计摄像机的运动（序列的其余部分被用于初始化），而基于点的方法只跟踪序列的34%。在第二个序列中，即ofkt-3，SVO在准确性方面显示出轻微的优势。然而，值得注意的是，它只能够跟踪57%的序列，因此，它不会受到在序列的困难部分引入的较高误差的影响。

在TUM数据集上的评估

我们还在TUM数据集[26]中评估了SVO和PL-SVO方法的性能，该数据集包括在不同环境中用RGB-D相机记录的几个序列，如图7所示。表二包含了TUM数据集中所考虑的序列的结果。总的来说，我们观察到PL-SVO在大多数序列中的优异表现，因此证实了它在多种环境中的稳健行为。然而，在这个数据集中，运动估计的准确性大大降低，单目受到运动模糊和其他由RGB-D传感器的滚动快门产生的负面影响的严重影响。

处理时间

最后，我们分析了将线段引入框架对处理时间的影响。表I显示了在算法的不同阶段所使用的平均时间。正如人们首先想到的，当包括线段时，执行稀疏图像和特征对齐所需的计算成本大大增加，每个阶段的运行时间增加了4毫秒。然而，我们的算法仍然以几乎60赫兹的频率实时执行，这取决于场景的类型。

结论

在本文中，我们通过将Forster等人在[1]中提出的SVO算法扩展到线段的情况，提出了一种新的单目方法。因此，我们获得了一个能够处理非纹理环境的更强大的系统，在这种环境中，基于点的方法的性能通常会由于难以找到分布良好的点集而恶化。半密集方法允许快速跟踪线段，从而消除了每当引入一个新帧时检测和匹配的必要性，这是采用这种类型特征的主要限制之一。我们在一系列合成和真实数据集的实验中验证了所声称的特征，证实了这一建议的稳健行为。

往期回顾

港大重磅SLAM新作！R3LIVE++：一个实时鲁棒的紧耦合激光雷达-惯性-视觉融合框架

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

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

加入我们：自动驾驶之心技术交流群汇总！

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！