VP-SLAM：具有点、线和灭点的单目实时VSLAM

00  摘要

传统的单目视觉SLAM（VSLAM）可以分为三类：使用特征的方法、基于图像本身的方法和混合模型。

在基于特征的方法中，新的研究已经发展到使用点以外的几何元素（如直线和平面）从环境中获取更多信息。这是因为在许多以“曼哈顿世界”为特征的人造环境中，几何元素（如线和平面）占据了环境中的大部分空间。利用这些方案可以引入能够优化视觉SLAM轨迹的算法，并帮助构建丰富的地图。为此，我们提出了一种实时单目视觉SLAM系统，该系统结合了线和VP提取的实时方法，以及两种利用消失点来估计机器人平移和改进其旋转的方案。特别的，我们以ORB-SLAM2为基础，该解决方案在精度和效率方面都被认为是当前最优秀的，并将其公式扩展到处理线条和VP，以创建两种策略：第一种优化旋转，第二种从已知旋转中对平移部分进行精细化。

首先，我们使用实时方法提取VP，并将其用于全局旋转优化。之后，我们提出了一种平移估计（translation estimation）方法，该方法利用末级旋转优化来建模线性系统。最后，我们在TUM RGB-D基准上评估了我们的方案，并证明所提出的系统实现了最优的结果并且可以实时运行，其性能与原始ORB-SLAM2系统保持接近。

注：VP(Vanishing Point，灭点)：在透视投影中，一束平行于投影面的平行线的投影可以保持平行，而不平行于投影面的平行线的投影会聚集到一个点，这个点称为灭点。灭点可以看作是无限远处的一点在投影面上的投影。又称“消失点”。

01  介绍

视觉SLAM（VSLAM）试图基于场景的多视图几何结构，结合计算机视觉算法来估计机器人的位置，同时生成环境的3D地图。它是3D重建、图像细化、3D全息应用、视觉位置识别、AR/VR现实、微型飞行器（MAV）自动驾驶等的关键内容。基于不同的传感器也产生了不同的视觉SLAM方案，如单摄像机、立体摄像机、RGB-D摄像机和事件摄像机。另外，基于特征的方法依旧最受关注，是因为它们依赖于非常适合的计算机视觉算法来提取特征，并且比直接方法更能抵抗光线变化。然而，在提取的点特征分布不均匀或不充分的低纹理或人造环境中，将多视图几何体中的其他几何元素（如线平面或VP）合并到SLAM系统中可以提高这些系统的鲁棒性。

另一方面，实践中的大多数应用程序都适用于某些场景，例如人造环境。在这些环境中，为了提高VSLAM系统的性能，使用了曼哈顿世界（MW）假设[5]。MW是一个具有显著结构规律性的人造环境，周围环境的大部分区域被描述为具有三个相互正交的主导方向的盒子世界。因此，每个MW与一个帧相关联，该帧被表示为曼哈顿帧（MF），并且可以从VP推断，VP是MW中3D平行线的图像投影的交点。因此，使用VP可以减少姿态漂移。

受上述见解的启发，我们提出了一个VSLAM系统，该系统集成了用于提取线条的简单计算机视觉算法和VP，以减少姿势漂移并对其进行优化。更具体地说，本文的主要贡献总结如下：

• 采用实时计算机视觉算法提取线条和VP..
• 基于VPs的绝对旋转优化策略。
• 用于估计变换的简单线性系统。

02  相关工作

接下来，我们简要回顾一下VSLAM的重点是功能和利用结构规则来提高系统性能的相关工作。首先，我们有ORB-SLAM2[8]，这是一个广泛使用的基于特征的单眼VSLAM系统，它扩展了PTAM的多线程和基于关键帧的架构[9]。它使用ORB特性构建可见性图（co-visibility graph ），执行回环检测和定位任务。为了提高基于点的方法的鲁棒性，作者在文献[9]中从环境中提取了线特征，并提出了一种将它们集成到单目扩展卡尔曼滤波器SLAM系统（EKF-SLAM）中的算法。最后，在PL-SLAM[1]中，将点和线同时提取到基于点的系统中。

另一方面，我们有基于MW假设的VSLAM系统，像文献[6]提出了基于VP和平行线的两阶段MF跟踪来估计姿态的旋转，并提出了姿态优化的精细化方案。文献[10] 提出了一种均值偏移算法来跟踪MF在场景中的旋转，同时使用一维密度对齐进行平移估计。OPVO[11]通过使用KLT跟踪改进了平移估计。这两种方法都要求两个平面在帧中始终可见。LPVO[12]通过将线段集成到系统来缓解这一要求。结构线与主要MF的轴对齐，并可以集成到均值偏移算法中，从而提高鲁棒性。因此，对于LPVO，考虑到线段的存在，场景中只需要一个平面可见。但是，平移估计仍可能发生漂移，因为它依赖于帧到帧的跟踪。

03  系统概述

提出的VSLAM解决了三大挑战：实时复杂性、利用正交性和并行性约束优化全局旋转，以及利用全局旋转信息进行简单的平移细化。因此，在下面的段落中，将简要介绍所提出的VPSLAM系统的每个模块。

拟议的VP-SLAM系统如“图1”所示，该系统基于ORB-SLAM2[8]架构。该系统由两个模块组成：前端和后端。前端负责实时VO，提供自我运动估计，对当前帧和关键帧决策进行局部优化，而后端负责地图表示、局部地图优化以及插入和剔除关键帧的策略。

图片

与文献[1]类似，在前端部分，点特征和线特征在每个RGB帧中并行输出。然后，类似于ORB-SLAM2[8]，我们使用匀速运动模型来获得初始姿态估计，然后使用点和线来优化它。之后，为了进一步优化旋转，我们提出了一种结合了提取的VP和关于线平行度信息的优化方法。然后，知道优化旋转的我们可以通过求解线性系统来细化平移，如文献[6]的方案。此外，为了表示第i帧相对于全局坐标系的相机姿态，我们使用符号(\mathtt R_\mathtt{(iw)},\mathtt t_\mathtt{iw}) 进行表示，其中\mathtt{R_{iw}} \in SO(3) 是旋转矩阵，\mathtt{t_{iw}}\in \mathbb R^{3xn} 是平移向量。

我们还使用\delta^{\text i}_{\text k} 来表示C_i 帧中VP的方向集合，并使用\mathtt{d_k} 作为MW中三个正交主导方向的集合。此外，我们将“[·]_x ”定义为3 x 3斜对称矩阵，并使用“~”表示等式。

3.1 实时复杂性

计算复杂性是VSLAM算法中需要解决的最重要问题之一。为了保持ORB-SLAM2[8]的实时特性，我们仔细选择并使用了快速的操作线提取和VPs估计方法。

1） 线：为了检测图像中的线段，我们使用稳定的LSD（线段检测器）检测器[13]，它是一个O(n) 线段检测器，其中n是图像中的像素数。为了描述，我们使用LBD（线描述子）描述子[14]来描述线段，该描述子依赖于线的局部外观和几何约束，同时保持实时复杂性和对图像伪影的鲁棒性。

2） VPs估计：为了提取VPs，我们采用了文献[15]的方法，该方法基于2条线，并确保执行时间为O(n)，并且提取的消失点是最佳且正交的MW VPs。该方法的主要思想是利用高斯球作为旋转的参数空间，以主点(x_0,y_0)^T 为球的中心。因此，3D中的两条平行线被投影到高斯球上，成为在一点相交的两个大圆。该点距球体原点的方向被视为候选消失点方向（v_1 ）。为了实现实时性能，创建了一个与图像平面相交的极坐标网格，该极坐标网格跨越高斯球体的一半纬度和经度，尺寸为90x360，精度为1°。因此，在图像平面中的一点相交的一对线按权重构成极坐标网格的相应单元：

图片

其中||l_0|| 和||l_1|| 代表两条线段的长度，\theta 是其夹角，score是每个极坐标网格单元上的累积分数。在创建第一个备选VPs （v_1 ）方向并考虑正交约束之后，第二个VP（v_2 ）必须位于与v_1 生成360个均匀分布的与备选VPs（{v_{i}^{2}}^{360}_{i=1} ）方向垂直的大圆上。给定第一（v_1 ）和第二（v_2 ）VPs方向之后，第三备选VPs方向可以通过每对v_1 和{v_{i}^{2}}^{360}_{i=1} ）的叉积获得。因此，给定VP的所有备选方向，从得分最高的集合（VPs）中计算VP的最佳估计，其中（VPs）假设集合的得分是属于三个相关VP方向的三个极性网格单元的得分之和。

3.2 绝对旋转优化

受文献[6]、[7]的启发，我们提出了一种策略，通过利用提取的VP和曼哈顿世界（emfMW）的三个相互正交的主导方向\mathtt{d_k=[d_i^1|d_i^2|d_i^3]} 的集合，进一步优化相机旋转C_i ，其中\mathtt{d_i}^j 是列向量。尤其是，在给定场景中具有一组3D平行线的图像时，这些线必须与MW中的主导方向\mathtt{d_k^i} 对齐。因此，给定图像中至少两个线簇，相关簇\pmb{\mathtt{s_i}} 中对应线的高斯球面上的大圆的法向量必须垂直于簇的主导方向。因此，对于第i个相机帧C_i ，我们可以使用最小化公式来计算当前帧中的一组主导方向\mathtt{d_k^i} ，如下所示：

图片

其中\mathbb{S\in R}^{3xn} ，n是簇中的行数。表示s列的法线\pmb{\mathtt{s_i}} 可以从\pmb{\mathtt{s_i=K^Tl_i}} 获得，其中\pmb{\mathtt{K}} 是相机矩阵； \pmb{\mathtt{l_i=(sp_i \times ep_i/||sp_i \times ep_i|| )}} ，其中，\pmb{\mathtt{sp_i,ep_i}} \in \mathbb{R}^2是图像平面上的线的端点。最后，随着使用SVD解决这个问题，我们获得了当前C_i 帧中的相互正交主导方向\pmb{\mathtt{d_k^i}} 的集合。而且由于VP的方向反映了当前C_i帧相对于MW的方向，并且根据等式（1）计算的初始集合\pmb{\mathtt{d_k}} （而不是如文献[7]中那样根据初始帧上的VP计算）表示初始帧C_0 相对于MW的方向，我们可以构造如下等式，该等式将估计的VP与初始相互正交方向集合连接起来：

因此，我们得出结论：

图片

其中，\delta \pmb{_{\mathtt{k}}^\mathtt{i}} 是\mathtt{VP_{VP_k^i}} 的消失方向（the Vanishing Direction，VD）。

图片

因此，为了进一步优化当前C_i 帧相对于条件等式（2）的绝对旋转\pmb{\text {R}_{\text {iw}}} ，我们定义以下成本函数以最小化：

图片

注意，如果初始帧C_0 没有至少两个具有足够行的簇，我们继续下一帧，直到找到满足条件的帧C_i 。此外，李代数中的\omega_i 是李群中\pmb{\text {R}_{\text {iw}}} 的映射。我们使用Levenberg–Marquardt（LM）优化来最小化成本函数\text {E}({\omega _\text {i}}) 。成本函数\text {E}({\omega _\text {i}}) 的雅可比数为：

图片

\pmb{\text {R}_{\text {iw}}} 的初始值是通过优化线的再投影误差和点的重新投影误差而获得的。

3.3 变换优化

在上面优化全局旋转\pmb{\text {R}_{\text {iw}}} 之后，我们可以使用此信息来构建线性系统，以解决平移问题。此外，全局旋转\pmb{\text {R}_{\text {iw}}}的可用性可以将初始非线性重投影误差简化为线性误差。具体而言，从：

图片

通过已知绝对旋转，我们得出：

图片

其中，[]^{(j)} 是向量的第j行。对公式（4）进行整理，可得：

图片

其中：

图片

图片

等式（5）是一个最小二乘问题，可以在RANSAC框架下求解，以便从当前C_i 帧的所有观测对中找到最大内层集，以求解以下正规方程：

图片

04  实验评估

为了验证所提出的SLAM系统在计算时间和准确性方面的性能，我们使用TUM RGB基准[10]在真实场景中将我们的SLAM与最优的ORB-SLAM2[8]进行了比较。所有实验均使用AMD Ryzen 5 2600 6核CPU进行。在本文中，我们旨在证明在MW中利用VP和主导方向进行姿态优化的有效性。为了公平比较，我们禁用了两个系统的回环检测功能。这是因为当回环检测模块启用时，两个系统将收敛到相同的轨迹并具有相同的绝对姿态误差，导致我们看不到我们方法的结果。

4.1 TUM RGB-D基准中的定位精度

我们在TUM-RGB-D数据集[16]上测试了我们的方法，该数据集由多个真实世界相机序列组成，其中包含了以全帧速率（30 Hz）记录的各种场景，如杂乱区域、不同复杂结构和纹理的场景，地面真实轨迹和分辨率为（640x480）的图像。表格1展示了对VO方法和SLAM方法的绝对平移RMSE（均方根误差）进行了比较，也比较了在TUM-RGBD基准中不同序列的估计轨迹[16]

图片

在使用基准测试提供的脚本计算ATE（绝对平移误差）误差之前，将所有轨迹与7DoF与地面实况对齐[16]。如表1所示，尽管禁用了回环检测模块，提出的VP-SLAM的精度也接近现有最优水平。这是因为我们着重关注了尽可能实时地保持VP-SLAM的性能，并同时利用环境几何结构中的附加信息。此外，与在后端使用非线性姿态图优化的ORB-SLAM2相比，我们提出了两个用于优化和精细化的简单可编程模块。此外，禁用回环检测模块可以在一定程度上减轻旋转累积误差，但如图2所示，我们的方法优化的轨迹接近ORB-SLAM2[8]。此外，在图2中，（左）我们看到了产生VP-SLAM的轨迹，与图2的ORB-SLAM2（右）形成对比。最后，在图3中，我们给出了提取VP后计算的线簇。

图片

图片

图片

4.2 时间复杂度

我们还根据最先进的ORB-SLAM2[8]评估了VP-SLAM的运行时间复杂读。在表2中，记录了VP-SLAM和ORB-SLAM2[8]中“Tracking”和“Local Mapping”中每个子任务所需的时间。请注意，最耗时的任务是两个系统中的局部BA和地图特征创建。总之，尽管线特征检测和VP提取结合优化策略增加了成本时间，但整个系统仍然可以实时执行。

05  结论

在本文中，我们提出了一种实时单目视觉SLAM系统，该系统利用人造环境的结构来进一步优化姿态。更具体地说，它特别适用于具有更多几何结构的环境，因为它可以从单个图像中检测VP和线特征。我们提出了两种方法，其中一种是利用VP的信息和线的并行约束来构建旋转优化策略，而第二种方法则使用前一种优化的信息来构建不同的变换细化策略。最后，在具有真实场景的基准数据集上的实验表明，所提出的系统的精度接近现有最优的ORB-SLAM2[8]。此外，性能保持实时性，并表明漂移可以进一步减少。

未来，我们打算用更强的优化技术来增强我们的系统，并在更多的室内环境中进行测试。

—— 精彩推荐 ——

1. 两万字 | 视觉SLAM研究综述与未来趋势讨论
2. 万字综述 | 自动驾驶多传感器融合感知
3. 末流985研一在读，视觉SLAM方向快坚持不下去了，大家可不可以给点建议...
4. 基于SLAM的机器人自主定位导航全流程
5. 17篇点云处理综述-点云语义分割、点云物体检测、自动驾驶中的点云处理……