基于点和线特征的实时单目SLAM

PL-SLAM: Real-Time Monocular Visual SLAM with Points and Lines

摘要：

众所周知，低纹理场景是依赖于对应点的几何计算机视觉的主要短板之一，尤其是视觉SLAM。但是，有许多环境，尽管纹理少，仍然可以可靠地估计基于线的几何图元，例如在城市中或者室内场景，或者所谓的“曼哈顿世界”，其中结构化边缘是主要的。本文中，我们提出了处理这种场景的解决办法。具体而言，我们是建立在ORB-SLAM之上的，该系统可能是目前在精度和效率上最先进的，我们将其形式扩展为同时处理点和线特征。我们提出的解决方案，在输入图像的大多数点特征消失时也可以工作，有趣的是，它可以跟踪从连续三帧中检测到的线特征进行初始化。我们完整的用TUM RGB-D基准评估了我们的方法和初始化方法，实验表明，线特征的使用在低纹理场景下，不仅可以提高原来ORB-SLAM的性能，还可以系统性的改进它在一序列图像帧中对点和线的结合，而不会影响效率。

1.引言

在过去几年间，无人驾驶汽车和无人机已经在不需要人为干预的情况下自主行驶了数百英里[10], [16], [32]。其他技术的核心也展示了同时定位与建图（SLAM）算法，已经证明在精确估计路径的同时，几何重建未知环境的有效性。
自从Klein和Murray在2007年引入了PTAM（Parallel Tracking And Mapping）算法[13]，其他很多实时的视觉SLAM方法也被提出，包括基于点特征的ORB-SLAM[18]，和基于直接法的LSD-SLAM[7]，以及直接优化图像像素的RGBD-SLAM[6]。其中，ORB-SLAM[18]似乎是最好的，精度优于直接法。
尽管ORB-SLAM[18]的性能在丰富纹理序列中的表现令人惊叹，但是在处理质感不佳的视频或特征点消失（例如因为运动模糊）时，很容易失败。这种场景在人为的场景中很常见。然而，尽管缺乏可靠的点特征，这些场景仍然包含了很多线特征，可以用类似的方法。
然而，提取线特征并不容易。首先，现有的线特征检测和参数化方法，不如点特征容易建立。其次，从对应的线特征中计算位姿的算法不如基于点特征的方法可靠，并且对空间遮挡很敏感。这些原因使得目前基于线特征的SLAM方法主要是在距离相机和激光扫描仪上[2],[12],[20],[25]。
本文中，我们使用纯视觉方法来处理所有这样的情况。基于ORB-SLAM[18]，我们提出了PL-SLAM（点和线特征SLAM），一个可以同时处理点和线特征信息的方法。如[30]所提出的，线特征用其端点参数化，它在图像平面上的精确定位是通过一个两步优化方法估计而来。这种表示，除了对闭塞和误检测提高鲁棒性，还允许将线特征表述加入到SLAM机制中，就跟点特征一样，因此可以重用ORB-SLAM的部分机制。所得到的方法被证实，在低纹理场景下，也能够有很好的精确性，此外，纹理丰富的场景中，能够提高原来ORB-SLAM的性能，如图1所示。

本文的另外一个贡献是，我们也提出了一个新的初始化方法，允许在三个连续图片中，只根据对应的线特征近似估计一个初始化地图。之前的解决方法是基于单应性矩阵[8]或者本质矩阵[29]，以及特征点对。据我们所知，没有类似的基于线特征的方法。我们提出的解决方案是，假定三个连续帧之间的旋转恒定，并且旋转相对较小。在实验部分，我们将展示，尽管我们估计的近似初始地图和基于特征点的相似，但是当特征点不可用时，它是非常好的选择。

2.相关工作

数十年来，从单目相机序列中建立未知环境的3D刚性结构的同时，恢复相机的路径，在机器人和计算机视觉中是一个极其重要的研究邻域，在自主机器人导航和虚拟现实中有很多应用。这个问题称为SLAM，核心与SFM算法大致相同。
早期的滤波器方法使用扩展卡尔曼滤波EKF[5]来处理视频的每一帧，从而建立小的地图，提供了最初的实时解决方案。随后的基于BA的稠密地图方法，只使用关键帧来估计地图[13][17]，获得的结果比滤波器方法更精确[27]。大部分方法依赖于PTAM算法[13]，代表了基于视觉的SLAM的突破。这种方法近似的解耦为定位和建图两个并行的线程，基于FAST角点[23]。在[14]中，边缘特征和跟踪过程中的旋转估计步骤的精度得到提升，从而提供了更好的定位结果，甚至减少了计算成本[24]。更近的，ORB-SLAM系统提供了一个更为鲁棒的相机跟踪和建图估计方法。一个多线程的CPU方法在[7]中展示，来估计一个实时的稠密结构。
然而，前面提到的基于特征的方法在低纹理环境下或者散焦和运动模糊的情况下失败。为了解决该问题，可以应用稠密和直接法，即使他们的计算量会很大[19],[21]，并且需要GPU来实现实时性能。其他的半稠密方法如[9]克服了稠密方法的高计算需求，通过探索高梯度的像素，提供中等层次的精度、稠密性和复杂性。场景先验信息也被用来为SLAM系统提供重要信息[3]，[4]。
受低纹理场景对有效精确的表达激励，在从空中飞行器或者手持设备（计算资源有限）的视觉视察任务中，我们提出了新的基于视觉的SLAM系统，它可以在统一的框架中结合点以及线信息，同时保持低计算成本。注意到，在EKF-SLAM[26]中，使用了多个参数化来组合点和线。然而，如上所述，在刚性SLAM中，基于优化的方法优于基于滤波的方法，正如我们在本文中所做的工作。我们在各种各样的情况下验证了我们的方法，在纹理丰富的场景中优于当前最好的解决方法，并且在低纹理场景下获得精确的结果，而标准的基于特征的方法失败了。

3.系统概览

我们的方法的流程与ORB-SLAM[18]的流程图非常相似，其中，我们整合了由线特征提供的信息，如图2所示。下面，我们将要简要的回顾以下流程图中主要的构建块。有关涉及到特征点的操作的描述，请参考[18]。

在SLAM算法中要解决的主要问题是计算复杂度。为了保持ORB-SLAM的实时性，我们已经精心选择、使用和实现了在流程各个阶段操作线特征的快速方法：检测、三角化、匹配、剔除、重定位和优化。输入帧中的线特征通过LSD[31]检测，一个计算复杂度为O(n)的线段检测器，其中n是图像的像素个数。然后，使用关系图策略[33]，将线特征与地图中已经存在的线特征进行匹配。这种方法依赖于线特征的局部外观（Line Band Descriptor）和几何约束，并且在保持计算效率的同时，显示出对图像伪影的鲁棒性。
由于它是通过点特征完成的，在获得初始的一组地图-图像的线特征对之后，局部地图的所有线特征被投影到图像上，以找到进一步的对应关系。然后，如果图像包含环境的充足的新信息，它就被设为关键帧，且它对应的线特征通过三角化加入到地图中。为了丢弃可能的异常点，观测到线特征的帧数少于3，或者在不到25%的帧中观察到的线特征也被丢弃（剔除）。地图中的线特征位置通过局部BA进行优化。注意到，在图2中，我们不用线特征来进行闭环。在全部地图中匹配线特征的计算量太大。因此，只用点特征用于闭环检测。

4.基于线特征的SLAM

接下来，我们将描述所使用的线特征参数化和误差函数，以及如何将其整合到SLAM流程中的主要构建模块中，也就是BA，全局重定位和特征匹配。

A.基于线特征的重投影误差

为了把ORB-SLAM[18]扩展到线特征，我们需要一个合适的重投影误差定义和线特征参数化。
如[30]中所述，让P,Q ∈ℝ3 ∈ R 3 作为一个线特征的3D端点，p d d ,q d∈ℝ2 d ∈ R 2 是图像平面上的2D检测点，而p hd d h ,q hd∈ℝ3 d h ∈ R 3 是对应的齐次坐标。从后者，我们获取归一化的线性系数如：

I=phd×qhd|phd×qhd|(1) I = p d h × q d h | p d h × q d h | ( 1 )

线特征的重投影误差 Eline E l i n e 被定义为投影线的端点到在图像平面上检测到的直线的距离 Epl E p l 之和，如图3右所示。为：

Eline(P,Q,I,θ,K)=E2pl(P,I,θ,K)+E2pl(Q,I,θ,K)(2) E l i n e ( P , Q , I , θ , K ) = E p l 2 ( P , I , θ , K ) + E p l 2 ( Q , I , θ , K ) ( 2 )

E2pl(P,I,θ,K)=ITπ(P,θ,K)(3) E p l 2 ( P , I , θ , K ) = I T π ( P , θ , K ) ( 3 )

其中， I是检测到的线参数， π(P,θ,K) π ( P , θ , K ) 表示端点 P在图像平面上的投影，相机内参 K已知，相机外参 θ={R,t} θ = { R , t } 包含了旋转和平移参数。

注意，在实际应用中会遇到线冲突或者误检测情况，图像平面上检测到的端点 pd p d 和 qd q d 和端点 P和 Q的投影不匹配，如图3左所示。这样，我们定义检测到的线特征重投影误差为：

Eline,d(pd,qd,I)=E2pl(pd,I)+E2pl(qd,I)(4) E l i n e , d ( p d , q d , I ) = E p l 2 ( p d , I ) + E p l 2 ( q d , I ) ( 4 )

其中， I是3D线特征的投影参数，检测到的点到线误差为 E2pl(pd,I)=ITpd E p l 2 ( p d , I ) = I T p d .
基于[30]中提到的方法，在检测到的重投影线特征误差的递归将被应用以优化位姿参数 θ θ ，同时近似 Eline,d E l i n e , d 到方程（2）中定义的 Eline E l i n e 。

B.点和线的BA

相机位姿参数 θ={R,t} θ = { R , t } 在每一帧都用BA进行优化， θ θ 约束在SE(3)李群中。为此，我们在ORB-SLAM框架上构建，但是包含特征点观测，我们包含了在前面定义的线特征。下面，我们定义具体的用BA优化我们提出的包含这两种几何元素的误差函数。
让 Xj∈ℝ3 X j ∈ R 3 作为地图中的第j个点。对于第i个关键帧，这个点可以这样投影到图像平面上：

x̃ i,j=π(Xj,θi,K)(5) x ~ i , j = π ( X j , θ i , K ) ( 5 )

其中， θi={Ri,ti} θ i = { R i , t i } 表示第i个关键帧的特定位姿。已知该点的观测值 xi,j x i , j ，我们定义3D误差为：

ei,j=xi,j−x̃ i,j(6) e i , j = x i , j − x ~ i , j ( 6 )

类似的，我们用 Pj P j 和 Qj Q j 表示地图上第j个线特征的端点。对应的投影到相同的关键帧上的图像投影点（表示为齐次坐标）可以写为：

p̃ hi,j=π(Pj,θi,K)(7) p ~ i , j h = π ( P j , θ i , K ) ( 7 )

q̃ hi,j=π(Qj,θi,K)(8) q ~ i , j h = π ( Q j , θ i , K ) ( 8 )

然后，给出第j个线特征的端点在图像上的观测点 pi.j p i . j 和 qi,j q i , j ，我们使用方程(1)来估计观测到的线特征 Ĩ i,j I ~ i , j 的参数。我们定义线特征的误差向量为：

e′i,j=(Ĩ i,j)T(K−1phi,j)(9) e i , j ′ = ( I ~ i , j ) T ( K − 1 p i , j h ) ( 9 )

e′′i,j=(Ĩ i,j)T(K−1qhi,j)(10) e i , j ′ ′ = ( I ~ i , j ) T ( K − 1 q i , j h ) ( 10 )

误差(9)(10)实际上是点到线的距离误差(3)。如[30]中解释的，端点 Pj P j 和 Qj Q j 沿着对应的3D直线的移动并不是固定的，作为隐式正则化，允许我们在BA中使用这种非最小直线参数化。
观察使用端点的线表示方法，我们获取点和线的可比较的误差表示。因此，我们可以建立一个统一的误差函数，将每个误差项整合为：

C=∑i，jρ(eTi.jΩ−1i,jei,j+e′i,jTΩ′i,j−1e′i,j+e′′i,jTΩ′′i,j−1e′′i,j) C = ∑ i ， j ρ ( e i . j T Ω i , j − 1 e i , j + e i , j ′ T Ω i , j ′ − 1 e i , j ′ + e i , j ′ ′ T Ω i , j ′ ′ − 1 e i , j ′ ′ )

其中， ρ ρ 是Huber核函数， Ωi,j,Ω′i,j,Ω′′i,j Ω i , j , Ω i , j ′ , Ω i , j ′ ′ 分别是检测到的点特征和线特征端点的协方差矩阵。

C.全局重定位

任何SLAM方法的一个很重要的部分是，当跟踪丢失时，相机重定位的方法。这通常是通过PnP算法来实现的，通过在先前关键帧中对应的3D地图点来估计当前丢失帧的位姿。在PnP方法之上，使用RANSAC方法来去除异常点。
在ORB-SLAM[18]中，PnP方法使用的是EPnP[1]，然而，只接受对应点特征作为输入。为了使我们的方法能够用线特征来重定位，我们把EPnP替代为最近新出的EPnPL[30]，它是最小化方程(4)中的检测线重投影误差。
此外，EPnPL对于线特征冲突和误检测很鲁棒。这通过两步过程来实现，首先使得检测到的线的重投影误差最小，然后估计线的端点 pd,qd p d , q d 。然后，这些点顺着这条线移动来匹配3D线段的端点P, Q的投影点 p̃ d,q̃ d p ~ d , q ~ d ，如图3所示。一旦建立了这些匹配，相机位姿就可以估计了。

5.基于线特征的地图初始化

本文的另一个贡献是，只使用对应的线特征来估计一个初始化地图。当前的基于优化的SLAM方法，用至少两帧之间的点对应建立的地图进行初始化。单应性矩阵[8]或者本质矩阵[29]估计算法用来计算初始化地图和位姿参数。下面介绍以下我们的基于线特征的地图初始化方法，在缺乏特征点的低纹理场景中，这可以作为一个好的选择。
现在考虑图4的建立，其中由端点P, Q定义的线段投影到三个相机图像帧中。用 {p1,q1},{p2,q2},{p3,q3} { p 1 , q 1 } , { p 2 , q 2 } , { p 3 , q 3 } 作为在每个视图中线段端点的投影点，用 I1,I2,I3∈ℝ3 I 1 , I 2 , I 3 ∈ R 3 表示从投影的端点计算而来的直线参数。
假设连续两个相机位姿之间小的且连续的旋转，这样使得从第一个相机视图到第二个相机视图的旋转和从第二个相机视图到第三个相机视图的旋转相同。在这个假设之下，我们可以表示这三个相机旋转为： R1=RT,R2=I,R3=R R 1 = R T , R 2 = I , R 3 = R ，I是3×3的单位矩阵。
注意，线系数 Ii,i={1,2,3} I i , i = { 1 , 2 , 3 } 也代表了向量的参数，该向量垂直于由投影中心 Oi O i 和投影点 pi,qi p i , q i 形成的平面。这样的两个向量 Ii I i 的叉乘将和直线P,Q平行，同时与第三个矢量正交，所有这些矢量经过适当的旋转并放入到公共参考系中。这个约束可以写为：

IT2((RTI1)×(RI3))=0(11) I 2 T ( ( R T I 1 ) × ( R I 3 ) ) = 0 ( 11 )

另外，对于小的旋转，我们近似 R为:

R=⎛⎝⎜⎜1r3−r2−r31r1r2−r11⎞⎠⎟⎟(12) R = ( 1 − r 3 r 2 r 3 1 − r 1 − r 2 r 1 1 ) ( 12 )

对于这个参数化，有三条匹配线，我们将会有三个含有三个未知数 r1,r2,r3 r 1 , r 2 , r 3 的二次方程，如方程11。我们调整[15]的多项式求解器，最多可以得到8个解。对每个可能的旋转矩阵，我们可以通过三焦张量方程[11]来获得 t1,t3 t 1 , t 3 。假设 t2=0 t 2 = 0 。我们评估8个可能解，然后获取使得方程11最小的那个解。
值得指出的是，为了在使用三焦点张量方程求解平移分量时，获得足够的独立约束，我们需要两个额外的线对应关系，因此，我们算法所需的线特征匹配数量是5。

6.实验结果

我们用TUM RGB-D基准数据[28]来比较了我们的方法和当前最好的视觉SLAM方法。另外，我们用合成的和实际数据评估了所提出的初始化方法，并比较了我们的PL-SLAM算法和ORB-SLAM算法的计算时间。所有的实验都在Intel Core i7-4790(4 Core @3.6 GHz），8G内存和Hydro ROS上进行。由于视频流某些阶段的随机性，例如初始化，位姿优化或者全局重定位，所有的实现均运行5次，然后取执行结果的中值。支撑材料可以参考网址：PL-SLAM

A.在TUM RGB-D基准中的定位精度

为了评估定位精度，我们比较了PL-SLAM方法和当前主流的视觉SLAM方法，包括ORB-SLAM[18]， PTAM[13], LSD-SLAM[7]和RGB-D SLAM[6]。用于比较的度量是由基准评估脚本提供的绝对轨迹误差(ATE)。计算误差之前，除了刚体转换对齐的RGB-D SLAM[6]之外，所有的轨迹都使用相似性扭曲对齐。结果总结在表1中。

注意到，我们的PL-SLAM在所有序列中都提高了ORB-SLAM的轨迹精度。事实上，除了两个序列之外，它产生了最好的结果，而那两个序列，PTAM表现略好。但是，PTAM[13]并不是这么可信，12个序列中有5个跟踪失败。LSD-SLAM和RGB-D SLAM也分别在第3和第7个序列中跟踪失败。

B.地图初始化-合成实验

为了评估我们在第5部分剔提出的地图初始化算法，我们执行了几个合成的和真实的实验。

在合成测试中，我们首先评估我们建立的多项式求解器的稳定性，修改了Kukelova等人的工具箱[15]。图5左图显示了理想解的参数估计误差分布。注意到，平均误差大概在1e-15左右，说明我们修改的求解器很稳定。
另外，我们已经评估了假定连续三帧之间的小旋转和恒定旋转的后果。图5右图展示了增加帧间旋转时产生的旋转和平移误差。尽管估计的旋转误差保持在相对较小的范围内，但小旋转假设对平移误差的影响更为严重。无论如何，当初始地图被反馈到BA优化中时，平移误差将会显著减小。

C.地图初始化——真实实验

我们还使用经典的初始化方法（基于单应性矩阵或者本质矩阵），以及仅基于线特征的地图初始化方法（参见表1）来评估我们的PL-SLAM方法。正如预想的那样，基于线特征的地图初始化的准确性由于它所做的小旋转假设而下降。然而，在低纹理序列 f3_nstr_tex_far f 3 _ n s t r _ t e x _ f a r 中，经典的初始化方法检测到一个模糊的地方，使其无法初始化地图。相比较之下，所提出的基于线特征的初始化方法能够估计初始地图。在序列 f3_walk_xyz f 3 _ w a l k _ x y z 和 f3walkhalfsph f 3 w a l k h a l f s p h 中，所提出的初始化方法不起作用，因为在初始帧中产生大的帧间旋转。

D.计算时间

当向视觉SLAM中添加了线特征，提高了精确性和鲁棒性的同时，也增加了计算复杂性。表2总结了PL-SLAM和ORB-SLAM在跟踪和局部建图模块的计算时间。注意，在子任务中，计算量大的地方是创建地图特征和局部BA。在任何情况下，PL-SLAM的最终帧率接近20帧/s，在标准没有优化的PC中。

7.总结与展望

本文中我们提出了PL-SLAM，该视觉SLAM方法允许同时处理点特征和线特征，并处理只用点特征容易失败的情况，例如低纹理场景或者运动模糊图片，其中特征点会消失。
1.我们在当前最好的ORB-SLAM架构基础之上建立，修改原有的流程图，使其能够处理线特征，同时不会对其效率产生太大影响。
2.提出了一个新的基于线特征的地图初始化方法，它是从三帧连续图像的5对线特征估计相机位姿和3D地图。该方法基于这三帧图像之间的旋转很小的假设。实验部分展示了该方法在很多场合的有效性。
3.另外，我们评估了完整的TUM RGB-D基准，显示出我们的方案一致优于当前主流方法。
在今后的工作中，我们计划进一步利用线特征，并结合平面这样的其他几何图元，这些图元可以通过类似于我们从点特征建立线特征的方法来建立。