基于相交线的立体平面SLAM
标题：Stereo Plane SLAM Based on Intersecting Lines
作者：Xiaoyu Zhang, Wei Wang*, Xianyu Qi and Ziwei Liao
来源：分享者
代码：https://github.com/fishmarch/Stereo-Plane-SLAM.git
欢迎各位加入免费知识星球，获取PDF论文，欢迎转发朋友圈分享快乐。
摘要
平面特征是SLAM系统中减小漂移误差的一种稳定标志。从密集点云中提取平面是一种简单、快速的方法，常用于RGB-D相机或激光雷达。但是对于立体相机来说，密集点云的精确高效计算是一个难点。本文提出了一种从立体图像中提取相交线计算平面参数的新方法。平面特征普遍存在于人造物体和构筑物的表面，具有规则的形状和直线的线条。在三维空间中，两条相交的直线可以确定这样一个平面。因此，我们从立体左、右图像中提取直线段。通过立体匹配，计算出三维空间中的端点和直线方向，进而计算出平面。在立体SLAM系统中加入这样的计算平面特征可以减小漂移误差，提高系统性能。我们在公共数据集上测试了我们提出的系统，并与最先进的SLAM系统进行了比较，证明了它的鲁棒性和精确性。
主要内容
本文提出了一种从立体图像中计算平面特征的新方法。平面特征普遍存在于人造物体和结构的表面。这些平面通常有规则的形状和直线。在三维空间几何中，两条相交的直线可以确定一个平面。因此，用直线来计算平面特征是合理和可行的。立体匹配可以从立体图像中计算出三维直线[4]。计算平面特征的示例如图1所示。与直接使用线特征相比，平面特征避免了复杂的参数化，实现了简单而稳健的数据关联。此外，平面也是更精确的地标，计算平面特征的过程也起到了滤除不精确线段的作用。
综上所述，我们的贡献如下：
·一种基于相交线的立体图像平面特征计算方法。
·使用提取点和计算平面的立体SLAM系统。
·在公共数据集上进行评估，系统得到了稳健和准确的估计结果，并实现了先进的性能。
相交线提取平面特征
本节主要介绍平面特征的计算方法。我们首先从两幅立体图像中提取线段。通过匹配直线段及其端点，计算直线端点和方向向量的三维位置。然后我们检查它们的位置，找出相交的线。最后，我们计算了平面参数
A 符号说明
我们将平面表示为p=（n^T,d）^T，其中n=（nx；ny；nz）^T是表示平面方向的单位法向量，d是平面到原点的距离。我们使用常用的形式Tcw属于se（3）来表示相机的姿势，p=（x,y,z,1）^T来表示点。因此，Tcw*pi_w将三维点从世界变换到摄影机坐标系，而Tcw^-T*pw将平面从世界变换到摄影机坐标系,对于直线，我们只记录它们的端点（ps；pe）和单位方向向量nl，这足以计算平面特征。
B 线段检测和计算
立体相机的帧由左图像和右图像组成。使用线段检测器(LSD)从两幅立体图像中提取线段，并用LBD描述子进行匹配。在一帧立体图像中，直线匹配具有足够的精确性和鲁棒性。如图2（a）所示，以不同的颜色绘制线段，并且在Il和Ir中匹配的线段是相同的颜色。对于左图像Il中的每一个匹配线段，假设它们在立体帧中的行位置不变，在右图像Ir中找到其端点的对应点。如图2（b）所示，匹配的端点通过横线连接。为了清晰起见，只绘制那些成功计算平面特征的端点。根据端点的立体匹配，基于视差delta_u计算出它们的3D位置p。线方向n_l也由其两个端点（pe−ps）定义
C 线段计算
在计算平面特征之前，需要检查直线之间的关系。在三维空间几何中，相交线或平行线位于同一平面上。然而，对于平行线，很难判断它们是否是从同一个真实平面提取的，因此由它们计算的平面容易带来较大的误差。因此，只计算相交线的平面。为了快速检查相交线，发现满足以下条件的直线：
·两条直线之间的角度大于阈值（在实验中为10°）
·它们的中心点之间的距离小于直线长度。
· 这两条直线的四个端点位于同一平面上。
中心点pc由直线端点ps和pe计算。从前两个条件中，实际上找到了那些紧密的线段用线方向向量的叉乘来计算平面法向量。
利用平面法向量n_pi和四个端点p_k（k=1,2,3,4），计算四个不同的平面系数d_k:
面与面之间的距离为
如果D小于阈值（在实验中为5cm），这两条线满足第三个条件，并且计算了平面系数pi,这里是d_k的算术平均值。有时计算的平面可能不是场景中的真实平面，例如门框线的平面。但是这样的平面也足够稳定，并且提供了精确的约束，因此我们将它们视为真实平面。在这些条件下，我们首先计算尽可能多的平面,稍后检查计算的平面并删除无效平面。
基于检测平面的SLAM系统
点和平面都被用作地标，并在我们的SLAM系统中得到优化。因为平面特征通常不能完全约束摄像机的姿态。我们的系统基于公开提供的ORB-SLAM stereo版本，其中包括特征跟踪和捆绑调整优化。
A、 系统概述
提出的SLAM系统的流程如图3所示。它可以分为帧处理、跟踪和建图三个部分。还没有添加回环检测部分，因为平面特征现在只对相机姿态跟踪有帮助。
在立体相机处理中，我们从左右图像中提取特征点和线段，并基于描述子对这些特征进行匹配。然后我们可以使用上述方法计算平面特征。在跟踪过程中，根据匹配的特征估计出摄像机的每个姿态。首先在最后一个关键帧对摄像机姿态进行估计，然后在局部地图上进行优化。从关键帧创建地图点和平面并将其保存在地图中。为了获得更精确的估计，进行了局部地图优化。
B、 优化公式
SLAM通常被描述为一个非线性最小二乘优化问题，而捆集平差法（BA）通常用于点特征。和点一样我们的系统设计了平面特征的优化公式。在我们的SLAM系统中，我们将摄像机的姿态、点特征和平面特征分别表示为C，P，L，则优化问题可以表述为：
e分别表示摄像机位姿、摄像机平面的测量误差。S是相应的协方差矩阵。优化问题可以用g2o中的LevenbergMarquardt或Gauss-Newton法求解
C、测量误差
1） 摄像机位姿点误差：在我们的系统中，我们使用标准的重投影误差来测量摄像机位姿。
这里Tcw是摄像机的姿态，pw是世界坐标系中的点参数，uc是观测到的像素，r是将3D点投影到图像上的摄像机模型。在优化中，摄像机姿态Tcw被映射到李代数x属于se(3)。
2）相机平面特征误差：由于三维平面只有三个自由度，因此pi参数化过多。因此，需要额外的约束来保证平面法向量的单位长度，在优化中增加了额外的计算量。为了克服这个问题，在优化中使用平面t的最小化参数，其中 括号里分别是平面法线的方位角和仰角：
所以我们定义最小化参数的测量误差方程：
这里pw是世界坐标系中的平面参数，pc是相机坐标系中的平面观测值。相机平面误差测量的是平面地标与其在相机坐标系中的相应观测值之间的距离。
实验
使用两个流行的公共数据集来评估提出的SLAM系统：EuRoC数据集和KITTI vision benchmark。这两个数据集都提供立体图像。所有的实验都在一台笔记本电脑上运行，它有 i7-7700HQ 2.80 GHz CPU, 16GB RAM, 没有GPU.
比较了不同SLAM系统的估计结果。这里我们使用绝对平移均方根误差（RMSE）来评估估计结果
EuRoC数据集与ORB_SLAM2的轨迹比较
EuRoC数据集构建的点云及平面地图
KITTI数据集其一轨迹比较
KITTI数据集的地图构建结果
总结
提出了一种从立体图像中计算平面特征的新方法。许多以前的工作已经证明了在SLAM系统中添加平面特征的好处，但大多数都是针对RGB-D相机的。在本文中，我们根据两条相交线决定一个平面的事实，从立体图像中计算平面特征。在进一步的验证之后，将计算出的平面加入到我们的立体SLAM系统中。我们给出了两个公共数据集的实验结果，证明了系统的准确性和鲁棒性。
从实验结果来看，我们的系统明显优于目前最先进的基于特征点的SLAM系统。与基于线的SLAM系统相比，我们的系统也得到了可比的结果。平面计算滤除了这些不精确的线段，并添加了稳定的约束来估计摄像机的姿态。从构建的地图中，发现一些不精确的平面特征仍然存在，这给数据关联带来了很大的挑战。未来，我们将对平面计算和检查方法进行改进，以获得更精确、更稳健的平面特征。此外，我们还需要一个更健壮的数据关联算法，消除估计误差的影响。
资源
三维点云论文及相关应用分享
【点云论文速读】基于激光雷达的里程计及3D点云地图中的定位方法
3D-MiniNet: 从点云中学习2D表示以实现快速有效的3D LIDAR语义分割（2020）
PCL中outofcore模块---基于核外八叉树的大规模点云的显示
更多文章可查看：点云学习历史文章大汇总
SLAM及AR相关分享