PL-SLAM：通过点和线段组合的立体SLAM系统

1. 引言

提出了PL-SLAM，这是一种立体视觉SLAM系统，它结合了点和线段，能够在更广泛的场景中稳定地工作，特别是在点特征在图像中缺乏或分布不均匀的情况下。PL-SLAM将点和线特征应用在slam全过程。通过线特征的长度和方向的比较，通过剔除异常值来解决特定于线段的跟踪和匹配问题，而对于残差计算，我们用线段的端点坐标表示地图中的线段。因此，通过图像平面上这些端点的投影与与被观测线段相关的无穷多条直线之间的距离来计算被观测线段与其在地图中的对应直线之间的残差。这样，我们就可以构建一个一致的成本函数，无缝地包含点和线特性。这两种特征也被用于在相机导航期间鲁棒地检测循环闭合，这是一种新的单词包（BoW）方法，它结合了使用它们执行位置识别的优点。综上所述，我们提出了一个新颖的多功能立体视觉SLAM系统coinedPL SLAM，该系统构建了[20]中提出的一种超曲面视觉测量（VO）方法，并结合了点和线段特征来执行实时摄像机定位和映射。
1） 第一个开源立体声SLAM系统，实时使用点和线段特性，因此能够在低纹理环境中稳健地运行，传统的点仅适用于ApproachestEndToFail。由于考虑到了这两种特征，我们的建议也产生了丰富的几何地图。
2） 束调整（BA）过程的一种新实现，该过程在优化关键帧（KF）的姿势的同时，无接缝地记录两种特征。
3） [21]中提出的BoW方法的一种扩展，它考虑了点和线段的描述，以改进环路闭合过程。

2. PL-SLAM框架

A. 地图

KFs包含观察到的立体特征及其描述符、通过视觉词汇计算的相应左侧图像的视觉描述符。对于地标，存储观测列表和每个检测到的地标最具代表性的描述符。对于特定的点，可以估计三维位置，而对于线段，我们既保留其方向，也保留其端点的估计三维坐标。最后，共视性信息由一个图来建模，其中每个节点代表一个KF，KF之间的边只有在共享最小数量的路标时才被创建（设置为20），允许本地地图的实时BA。为了实现更快的循环闭合优化，还形成了本质图，其密度小于共视性图作为两个KF之间的一条边，只有当它们共享超过100个里程碑观测值时才会创建。最后，该映射还包含一个生成树，它表示包含所有kf的图的最小连通表示。

B. 特征跟踪

从一系列立体图像帧中跟踪图像特征（点和段），并计算它们的三维位置及其由协方差矩阵表示的相关不确定性。然后将三维地标投影到新的摄像机姿态，并且最小化投影误差，以获得与该估计相关的摄像机姿势增量和协方差。这个过程在每一个新的帧中重复，只执行帧到帧的VO，直到一个新的KF被插入到映射中。
**点特征：**对于特征点的处理，由于其高效性和良好的性能，我们采用了ORB检测器和描述符。为了减少异常值的数量，我们只考虑互为最佳匹配的测量值，即左图像中的最佳匹配对应于右图像中的最佳匹配。为了保证对应关系足够有意义，还检查了描述空间中两个最近匹配之间的距离是否高于某个阈值，该阈值设置为最佳匹配距离的两倍。还使用bucketing方法确保在输入图像上公平分布点，该方法将图像分成16个桶，并尝试在每个桶中添加至少20个特征。
**线特征：**采用直线段检测器（LSD），具有较高的精度和重复性。然而，它耗时，这是它在实时应用方面的主要弱点。为了缓解这种情况，我们在两幅立体图像中以并行框架检测线段。对于立体匹配和帧到帧跟踪，我们首先计算每行的LBD描述符，然后根据它们的局部外观特征进行匹配。类似于点的情况，我们检查两个特征是否相互最佳匹配，以及最好的两个匹配在描述空间中是否充分分离。我们利用线段所提供的有用的几何信息来过滤掉那些方向和长度不同的直线匹配，以及那些在端点差异上有很大差异的直线匹配。
运动估计
一旦建立了两个立体帧之间的对应关系，就把关键点和线段从第一帧投影到下一帧。然后通过一个鲁棒的高斯-牛顿最小化线和关键点投影误差迭代估计摄像机的自我运动。为了处理异常值，使用了一个伪Huber损失函数并执行了两步最小化。最后得到两个连续帧之间的增量运动估计，可以用以下正态分布来建模：

式中ξt，t+1∈se（3）是帧t和t+1之间摄像机运动的6维矢量，∑ξ*t，t+1表示估计运动的协方差
关键帧选择
通过以下表达式将协方差矩阵的不确定性转化为标量，即熵：

对于给定的KF，我们检查前一个KF i与当前KF i+u之间的运动估计熵之间的比率，以及先前KF i与其第一个连续帧i+1之间的熵之比，即α:

如果α值低于某个预先设定的阈值（在我们的实验中设置为0.9），则帧i+u作为一个新的KF被插入到系统中。注意，要计算（2）中的表达式，我们需要非连续帧之间姿势增量的不确定性。

C. 局部映射

局部映射过程在共视性图中寻找新的KF、最后一个和连接到最后一个KF的新特征对应。这样，我们就建立了所谓的当前KF的本地地图，其中包括与当前KF共享至少20个地标观测的所有KF以及它们观测到的所有地标。最后，对局部地图中的所有元素（即KF姿态和地标位置）进行优化。

1. 关键帧插入

改进了当前KFs和先前KFs之间相对姿态变化的估计，一旦计算出KFs之间的相对姿态变化，将当前的KFs插入系统，包括以下内容：
1） 关键帧的索引。
2） 它的三维姿态信息，包括绝对姿态和相对姿态，以及它们的相关不确定性。
3） 新的三维地标，通过存储其二维图像坐标和描述符来初始化。已经存在的地标的新观测也被添加到地图中。

2. 局部BA

定义包含要优化的变量的向量ψ，即每个KFξiw的se（3）位姿，每个点Xwj的三维位置，以及每个线段端点的三维位置：{Pwk，Qwk}。然后，我们将观测值与投影到被观测帧上的地标之间的投影误差最小化:

投影误差Ei代表第i KF中第j个地图点观测之间的二维距离，可以表示为:

函数π：se（3）×R3？→r2首先将第j个三维点Xwj（在世界坐标系中）放入第i个KF的局部参考系，即Xij，然后将该点投影到图像中。

线段的使用略有不同，因为不能简单地比较端点的位置，因为它们可能会沿直线移动或从一帧遮挡到下一帧。为此，将三维线段的投影端点与其在像平面上对应的无限直线之间的距离作为误差函数。在这种情况下，在第i帧中观察到的第k条线之间的误差eik由下式给出:

式中Pwkand qwk为世界坐标系中线段的三维端点，lik为第i KF中第k条线段对应的无穷大线方程，由齐次坐标下线段的二维端点之间的叉积得到，即lik=pik×qik。
投影误差最小化可以通过采用Levenberg-Marquardt优化方法迭代求解，为此需要同时估计雅可比矩阵和Hessian矩阵，

误差向量e包含所有投影误差eij和eik，每个观测误差eijor eik，只依赖于一个KFξiw和一个地标Xwjor{Pwk，Qwk}。因此，黑森矩阵可以通过将每个观测值的影响加入到其相应的块中来形成。

最后，从地图上删除那些少于三次观测的地标。

D. 环路闭合

环路闭合检测是通过提取每个图像的视觉描述符来执行的，该方法基于BoW方法。采用了一种基于关键点和线段的二元描述子的BoW方法，以稳健地处理数据关联和循环闭合检测。摄像机运动期间捕获的KFs的所有视觉描述符存储在数据库中，它后来被用来寻找与最好的匹配将被认为是最合适的-仅当围绕这个KF的局部序列也相似时，才有可能得到闭包。最后，估计当前KF与循环闭合候选者之间的相对SE（3）变换，以便在找到正确估计的情况下，通过位姿图优化（PGO）过程校正回路中涉及的所有KFs姿态。

1） 视觉位置识别

图4（a）中的矩阵仅使用ORB关键点来构建词汇表和数据库，而图4（b）中的矩阵仅依赖于线。调色板从蓝色（分数=0）变为红色（分数=1）。可以注意到，一些淡黄色区域出现在第一个矩阵中，根据关键点图像看起来相似（特别是在走廊拐角处转弯后）。这表明了潜在的循环闭包，尽管实际上，它们只是误报。然而，第二个（仅线）矩阵不呈现这种行为，因此可以使用它来丢弃它们。另一方面，第一个矩阵表现出更明显的特征，因为对于非相似图像，分数的差异通常比在纯线矩阵中更大。因此，在检测潜在的循环闭包时，通过查询两个特征数据库得到的图像相似性可以结合起来以提高鲁棒性。

根据强度和分散性这两个标准对两个特征（skfor keypoints和slfor line）的结果进行加权。前者根据图像中检测到的特征集合中某类特征（关键点或直线）的数量按比例加权相似度，而后者考虑了图像中特征的分散性（越分散权重越高）。这将产生一个更稳健的图像总相似度得分（st）：

nk和nl分别是在图像中提取的关键点和线条的数量，而关键点和线条的分散值（分别为dk和dl）被计算为所发现特征的x和y坐标中方差之和的平方根。对于直线，这样的x和y坐标是从它们的中点开始的。

2） 估计相对运动

查找两个KF的特征之间的匹配，同时还搜索当前KF和旧KF，根据运动估计中描述的方法估计了一个有效的变换ˆξij∈se（3），它将两KFs联系起来，通过以下测试检查循环闭合候选的一致性：
1） 协方差矩阵∑ξij的最大特征值小于0.01。
2） 获得的平移和旋转不能分别超过0.50 m和3.00°以上。
3） 估计的输入比大于50%。

B、 回路校正
在估计了轨迹中所有连续的循环闭包之后，我们再融合循环闭合的两侧，以修正沿回路分布的误差。这通常通过将问题描述为一个PGO来解决，其中节点是循环中的KFs，并且边由本质图和生成树给出。为此，将以下误差函数定义为将KFsˆξij与同一变换的当前观测值相关的转换之间的se（3）差：

ξij的值与优化的第一步中ξij的估计值一致，因此这些边的误差最初为零。
当考虑循环闭合边时，使用g2Olibrary[50]解决了这个问题，得到了优化中包含的KFs的最佳姿势，即基本图和生成树。最后，我们根据KFs观测到的地标的姿态更新KFs的姿态，并通过首先融合匹配的地标，同时估计它们的相对运动，然后在不匹配的地标之间寻找新的对应，从而合并环路两侧的局部地图。