CN110782494A - 一种基于点线融合的视觉slam方法 - Google Patents

Abstract

一种基于点线融合的视觉SLAM方法，首先输入一幅图像，预测相机位姿，对图像提取特征点，利用多个视角间的时序信息估计并提取特征线。匹配特征点和特征线，在前后帧跟踪特征，建立帧间的关联，然后对当前帧的位姿进行优化，并优化二维特征线，以提升特征线的完整度。判断当前是否为关键帧，对于关键帧，则加入地图中，并更新地图中的三维点和线，对当前关键帧和相邻的关键帧进行联合优化，优化相机的位姿和三维特征。并剔除部分外点，剔除冗余的关键帧。最后对关键帧进行回环检测，若当前关键帧和之前帧为相似场景，则闭合回环，并进行一次全局优化，消除累积误差。本发明在基于点和线的SLAM系统框架下，利用多个视角图像的时序关系，提升线条提取的速度和特征线的完整度，从而提升位姿精度和地图重建效果。

Description

一种基于点线融合的视觉SLAM方法

技术领域

本发明属于图像处理、计算机图形学和计算机视觉等交叉学科，涉及视觉同步定位与地图构建(SLAM)领域。

背景技术

同时定位与地图构建(简称SLAM，Simultaneous Location And Mapping)是一种在未知环境中，进行自身定位并同时建立地图的技术。主要应用在移动机器人、自动驾驶、虚拟现实、增强现实等方面。视觉SLAM主要使用视觉传感器作为输入设备，比如单目相机，双目相机，深度相机等，系统将图像作为输入，输出相机轨迹以及重建的三维地图。

主流的视觉SLAM通过对图像提取特征点，利用特征点与相机的位姿以及特征点的三维坐标之间的几何关系，求解相机轨迹和三维地图，比如Raul等人2017年在Robotics上发表的“ORB-SLAM2:An Open-Source SLAM System for Monocular,Stereo,and RGB-DCameras”方法。但点特征在光照变化、噪声干扰、运动模糊、弱纹理等场景中表现不佳。所以有些系统采用点和线融合的方式求解SLAM问题。目前较新的系统大多采用LSD、EdLines等方法在图像中提取直线段，比如Pumarola等人在2017年的ICRA会议上提出的“PL-SLAM:Real-time monocular visual SLAM with points and lines”方法，该方法在ORB-SLAM2的基础上增加特征线的处理。本发明方法充分利用多个视角之间的时序关系，使用时序关系提取特征线，提升线特征的帧间连续性和鲁棒性，可以在一定程度上应对遮挡、断裂等情况。

在基于关键帧的SLAM系统中，系统需要建立重投影误差来优化相机位姿。点的重投影误差一般采用投影点和观测点的欧式距离度量，而线的误差计算方式一般分为两种，一种采用两条直线的端点到端点的欧式距离作为误差值，比如早期的Gee等发表在2006年的Advances in Visual Computing的“Real-time model-based SLAM using linesegments”算法，该算法很方便的从点特征误差计算方式扩展到线特征上，而另一种采用观测线的端点到投影线的垂直距离作为误差值，目前基于直线的SLAM大多采用此方式计算。本发明利用端点到重投影直线的垂直距离作为重投影误差优化相机的位姿，在完成位姿优化之后，根据二维特征线的时序关系对提取的特征线的端点进行优化，以提升线段观测的完整度，从而更好的预测线条，同时也提升地图重建效果。

发明内容

SLAM的任务目标是尽可能地提升位姿精度和建图效果。本发明提出一种基于点线融合的视觉SLAM方法。该方法利用帧间时序性关系，更好地提取特征信息，并在位姿优化之后，反向优化二维特征线，从而提高位姿精度，提升地图重建效果。具体的实施步骤如下：

步骤1，输入图像；

步骤2，特征跟踪；

步骤2.1，预测相机位姿，并提取特征点和特征线；

步骤2.2，匹配前后帧的特征点和特征线；

步骤2.3，优化相机位姿；

步骤2.4，优化二维特征线；

步骤2.5，判断是否设为关键帧。若为关键帧，则将其加入地图，否则返回步骤1；

步骤3，局部建图；

步骤3.1，剔除冗余特征并插入新特征信息；

步骤3.2，优化局部地图；

步骤3.3，裁剪冗余关键帧；

步骤4，回环检测；

步骤4.1，检测是否形成回环；

步骤4.2，闭合回环；

步骤4.3，优化全局地图，消除累积误差。

与现有方法相比，本发明所采用的方法具有以下优点：在特征线提取方面，线段被提取得更完整，提取速度得以提升，线段在帧间的连续性较原有方法更好。在位姿计算方面，位姿精度得以提升。最终对地图重建效果起到积极作用。

附图说明

图1本发明方法所涉及的整体流程图。

图2视觉SLAM求解问题的数学建模。(a)通用SLAM的贝叶斯模型，(b)本发明方法所采用的贝叶斯模型。

图3本发明方法与ORB-SLAM2方法在EuRoC数据集的运行结果对比。每个序列中精度较高的记录使用红色标出。

图4本发明方法在EuRoC数据集的重建效果。(a)图为序列中的某一帧图像。(b)为该图像所观测视角下的重建结果。

具体实施方式

以下将结合附图对本发明做进一步说明。

我们设某个三维路标为X，相机在0,1,…,t-2,t-1,t时刻拍摄了t+1张图像，则最近三帧图像，所对应的相机位姿可表示为T_t-2,T_t-1,T_t，该三维路标X所对应的特征点为x_t-2,x_t-1,x_t。那么，通用的视觉SLAM问题的数学模型如图2(a)所示，其贝叶斯概率表示为P(x_t|T_t,X)。而本发明所建模的数学问题，增加了帧间时序性关系的表示，如图2(b)所示，其特征观测模型的贝叶斯概率表示为P(x_t|T_t,X,x_t-1)。

本发明基于该数学模型确定了如下计算流程。整个系统计算流程如图1所示，分为三大模块：特征跟踪、局部建图、回环闭合。系统数据采用全局地图、局部地图、共视图三个结构辅助记录。全局地图存储所有三维路标以及每一个关键帧对应的相机位姿和二维观测信息；局部地图是全局地图的子集，记录了全局地图中和当前关键帧相邻的关键帧所包含的信息；而共视图包含了关键帧之间的关联，如果两个关键帧观测的三维路标存在交集，则说明该帧之间存在关联。下面对每一步进行详细阐述，该思想可适用于单目或双目系统。

步骤1，输入图像

使用相机拍摄图像，并将每一帧图像依次输入系统中。若相机未标定，可以使用相机在不同的视角下对棋盘格拍摄多张图像，采用张正友标定法计算相机参数。

步骤2，跟踪特征

步骤2.1，预测位姿并提取特征点和特征线

根据前两帧的相机位姿，预测当前帧的相机位姿T_t：

其中，

为第t-2帧相机到第t-1帧相机的相对位姿，我们假设帧间运动是平缓的，并将该项作为第t-1帧到第t帧的相对位姿的估计值。由此得出当前帧的相机位姿。

然后提取图像中的特征点，同时获得特征描述子。本发明采用Raul等人的ORB-SLAM2中的特征点提取算法，该算法对ORB特征点提取算法做了改良，使得提取结果更佳。我们将特征点集合记为p_t。

最后利用当前帧的特征线的时序关系和预测的相机位姿辅助提取图像中的特征线。对于第一帧图像，我们采用Refeal等人2010年在Software Engineering上发表的“LSD:A Fast Line Segment Detector with a False Detection Control”中所介绍的LSD算法对图像提取特征线。对于初始化或重定位之后的5帧图像，由于其特征线在前后帧之间的时序关系建立得不完善，因此本方法也采取LSD算法直接提取特征线。对于之后输入的图像，本方法利用当时已经存在的特征线序列辅助提取线段，每个特征线序列包含至少N_l条分别来自不同图像的二维线段，这些线段来自于同一条三维线段的不同投影。对于上一帧得到的每一条特征线，本方法估计线段的每个参数相对于时间的变化量

为了更准确有效的预测线段在二维平面的运动规律，我们使用特征线的长度、角度、中心点的X和Y坐标四个参数值描述一条线段，那么在当前图像中该线段的预测结果为：

其中，l_t-1为上一幅图像中的二维线段。为提高预测的准确度，我们同时使用重投影的方式对线段进行预测。我们记l_t-1反投影的三维线段为L，那么当前图像上的二维线段l_t也应该和三维线段L存在投影关系。因此，根据投影几何关系，预测当前图像中该线段为：

最后对两种预测结果进行融合，得到最终的预测结果：

单纯地使用预测的方式获得的线条存在一定的误差，并且会随着时间推移而累积下来。因此，本发明在预测结果上采样最少N_p个点作为种子，并在图像中提取多条直线，最终将合并后的直线作为最终的提取结果。本发明实例中将N_p设为10。

步骤2.2，匹配前后帧的特征点和特征线

图像中特征点的数量比较大，如果采用暴力匹配方式，计算量太大，会严重影响实时性。因此，利用局部搜索加快匹配速度，首先由前后帧的几何关系，可知

其中，π_p表示点的投影函数。将p_t-1反投影到三维空间中，再投影到当前图像，得到p_t-1对应特征点的估计位置p′_t。然后在以p′_t为中心，5×5的窗口内搜索匹配点。最终使用双向匹配策略，将互为最佳匹配点的特征集合作为匹配结果。

同理，对于特征线，本发明使用类似的方法。将上一幅图像中的特征线重投影到当前图像中，得到估计的特征线

其中，π_l表示线的投影函数。然后以一定的角度范围作为搜索窗口，匹配二维特征线。最终得到匹配特征线的集合。同时，也根据匹配结果更新特征线序列，将匹配失败的特征线序列从当前帧的记录中移除，将新检测到的特征线序列加入当前帧。

步骤2.3，优化相机位姿

在新图像的几何特征和上一幅图像进行关联之后，对步骤2.1中估计得到的相机位姿，进行优化，提升位姿的准确度。由上文的推导过程，以及公式(6)和公式(7)可知，重投影得到的特征点p′_t应尽可能和匹配的特征点p_t重合，特征点的重投影误差采用两点的欧式距离度量；而特征线l′_t也应和匹配的特征线l_t重合，特征线的重投影误差采用端点到直线的距离进行度量。相比于直接使用端点到端点的欧氏距离作为重投影误差，该方法在遮挡、提取不完整等情况下，端点到直线的距离不会发现变化，重投影误差的测量值基本不受影响，而端点到端点的方式在上述情况下，其重投影误差会明显的增加，导致位姿优化时不能很好地收敛到正确位置。相机位姿的优化可以通过最小化重投影误差实现。本发明融合点和线定义位姿优化的最小二乘问题：

其中，e_p和e_l分别为特征点和特征线的重投影误差，其计算公式为：

e_p＝p_t-π_p(p_t-1,T_t-1) (9)

式(10)中，

和

分别为直线l_t的两个端点的齐次坐标形式，从而计算出直线l_t的端点到重投影直线的距离之和。式(8)中，p^p为特征点的置信度，其表示为：

其中，n为该特征被检测到的次数，t₁为可调系数,Σ_p为该特征点的协方差矩阵，该矩阵和特征点所在ORB金字塔的级别直接相关。相应地，特征线的置信度p^l为：

其中，σ为特征线长度分布的方差值，t₂和t₃为可调系数，Σ_l为该特征线的协方差矩阵。迭代优化式(8)，直到误差收敛，所得结果即为当前帧的相机位姿。

步骤2.4，优化二维特征线

由于在步骤2.1中提取到的特征线将被用于下一幅图像的特征线预测阶段，所以，保证当前图像中特征线准确度是极其重要的一步。在步骤2.3中，我们采用了端点到直线的距离度量线段的重投影误差，这种计算方式能够很好地度量直线的方向和位置是否重合，但对于线段的端点是否重合并不敏感，因此，本发明针对二维线段的端点进行优化，以尽可能消除遮挡、弱纹理等导致的线条不完整问题。

在此步骤中，本发明依然采用重投影的方式对线段进行优化矫正。首先，我们将每个特征线序列中的线段反投影到三维空间中

得到三维线段L′_i,i表示某一个时刻。我们假设线段的端点服从高斯分布，分别统计两个端点的方差σ_L和均值

然后依据σ_L和

对三维线段进行调整，

其中，α为优化系数，决定单次优化的力度。换言之，L′_i和均值的距离越大，优化力度越大；而端点分布的方差越大，优化力度越小。最后将优化后的线段投影到图像平面上

完成对二维特征线的优化。

步骤2.5，判断关键帧

对该帧进行判断，是否将该帧作为关键帧加入地图并进行后续计算。为了能够更好的跟踪到几何特征信息来优化相机位姿，本发明方法做如下决策：

步骤2.5.1，插入关键帧之后已处理多于10帧图像，且当前局部建图模块处于空闲状态，则判断为关键帧；

步骤2.5.2，上一次插入关键帧之后已处理超过20帧图像，则判断为关键帧；

步骤2.5.3，跟踪丢失并重定位后，已处理图像不得少于20帧，否则不能作为关键帧；

步骤2.5.4，当前图像上匹配的特征点不少于30个，且匹配的特征线不少于5条，否则不能作为关键帧；

步骤2.5.5，当前图像上跟踪的特征相比最近关键帧上跟踪的特征少于90％，则判断为关键帧；

若当前帧判断为关键帧，则生成关键帧并传递给局部建图模块以进行后续计算。当前跟踪模块返回步骤1，继续处理下一幅图像。

步骤3，局部建图

步骤3.1，剔除冗余特征并插入新特征信息

对于新插入的关键帧，首先更新共视图和生成树，加入新关键帧节点。然后对局部地图中的三维特征进行裁剪和更新，由于被观测次数太少的特征有极大的可能是外点，所以裁剪掉三维地图中被观测次数少于三次的特征点和特征线，最后将该关键帧中匹配的新特征，使用三角化方法恢复特征深度信息，生成新三维特征并插入到地图中。

步骤3.2，优化局部地图

完整地图更新之后，对当前关键帧和与之关联的关键帧进行联合优化，与步骤2.3中优化当前帧的位姿相比，此步骤的主要优化对象为相关关键帧的相机位姿以及三维特征坐标。优化过程中所采用的重投影误差表达式和式(8)相同。在特征匹配时，不可避免地会引入一些外点，对相机位姿优化造成一定影响。因此，在迭代优化10次后，将重投影误差过大的外点移除，再继续迭代优化，如此重复4次，以尽可能地清除外点，并最大程度地优化相机位姿。

步骤3.3，裁剪冗余关键帧

这一步骤的必要性在于，步骤2.5中，系统尽可能多地插入新关键帧，以增加位姿优化时的几何约束，但过多的关键帧会导致地图中数据量增加，加重了计算负担。因此，系统将关键帧之间特征的重合度大于90％的关键帧剔除，以精简共视图。

步骤4，回环检测

步骤4.1，检测是否形成回环

首先，计算当前关键帧和相邻关键帧之间的词袋相似度，记相似度最小值为s_min，然后在运行时所维护的关键帧数据库中查询，若不存在相似度小于s_min的关键帧，则返回处理下一个关键帧；反之，则视为检测到回环。

步骤4.2，闭合回环

我们使用随机采样一致性算法计算两个关键帧之间的相似变换，然后将相似变换应用到当前关键帧与相邻关键帧上，完成闭环融合。

步骤4.3，优化全局地图

对所有关键帧和三维特征进行优化，以消除累积误差，优化过程中，重投影误差的计算方式采用公式(8)所示内容。

重复以上步骤，直至处理完全部图像。以Raul等人2017年在Robotics上发表的“ORB-SLAM2:An Open-Source SLAM System for Monocular,Stereo,and RGB-D Cameras”方法作为对比，说明本发明的有效性。实验环境为Ubuntu系统，i7处理器，16GB内存。图3展示了在EuRoC数据集上的绝对轨迹误差，在大多数图像序列上本发明方法比ORB-SLAM2方法精度更高。同时，我们截取了序列中某一帧的重建结果进行展示。重建效果如图4所示，图4(a)为序列中的某一幅输入图像，图4(b)为在该图像视角下，对场景重建结果的展示。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (5)

1.一种基于点线融合的视觉SLAM方法，其特征在于：利用帧间时序性关系提取特征信息进行地图重建；设某个三维路标为X，相机在0,1,…,t-2,t-1,t时刻拍摄了t+1张图像，则最近三帧图像所对应的相机位姿可表示为T_t-2,T_t-1,T_t，该三维路标X所对应的特征点为x_t-2,x_t-1,x_t，将帧间时序性关系与通用的视觉SLAM问题的数学模型相结合，得到贝叶斯概率表示为P(x_t|T_t,X,x_t-1)的特征观测模型；基于得到的贝叶斯概率表示为P(x_t|T_t,X,x_t-1)的特征观测模型确定计算流程；整个系统的计算流程分为特征跟踪、局部建图、回环闭合三个模块。

2.根据权利要求1所述的一种基于点线融合的视觉SLAM方法，其特征在于：利用帧间时序性关系提取及优化特征信息进行地图重建的具体步骤为：

步骤1，输入图像:使用相机拍摄图像，并将每一帧图像依次输入系统中；

步骤2，特征跟踪:

步骤2.1，预测相机位姿，并提取特征点和特征线:根据前两帧的相机位姿，预测当前帧的相机位姿T_t：

其中，

为第t-2帧相机到第t-1帧相机的相对位姿，假设帧间运动是平缓的，并将该项作为第t-1帧到第t帧的相对位姿的估计值,由此得出当前帧的相机位姿；提取图像中的特征点，同时获得特征描述子,将特征点集合记为p_t；利用当前帧的特征线的时序关系和预测的相机位姿辅助提取图像中的特征线；对于第一帧图像和初始化或重定位之后的5帧图像，采用LSD算法对图像提取特征线；对于之后输入的图像，利用当时已经存在的特征线序列辅助提取线段，每个特征线序N_l条分别来自不同图像的二维线段，这些线段来自于同一条三维线段的不同投影；对于上一帧得到的每一条特征线，线段的每个参数相对于时间的变化量为使用特征线的长度、角度、中心点的X和Y坐标四个参数值描述一条线段，那么在当前图像中该线段的预测结果为：

其中，l_t-1为上一幅图像中的二维线段，同时使用重投影的方式对线段进行预测；记l_t-1反投影的三维线段为L，根据投影几何关系，预测当前图像中该线段为：

最后对两种预测结果进行融合，得到最终的预测结果：

在预测结果上采样最少N_p个点作为种子，并在图像中提取多条直线，将合并后的直线作为最终的提取结果；

步骤2.2，匹配前后帧的特征点和特征线：对于特征点，利用局部搜索加快匹配速度，首先由前后帧的几何关系，可知

其中，π_p表示点的投影函数；将p_t-1反投影到三维空间中，再投影到当前图像，得到p_t-1对应特征点的估计位置p′_t；然后在以p′_t为中心，5×5的窗口内搜索匹配点；最终使用双向匹配策略，将互为最佳匹配点的特征集合作为匹配结果；

对于特征线，将上一幅图像中的特征线重投影到当前图像中，得到估计的特征线

其中，π_l表示线的投影函数；然后以一定的角度范围作为搜索窗口，匹配二维特征线；最终得到匹配特征线的集合；同时，也根据匹配结果更新特征线序列，将匹配失败的特征线序列从当前帧的记录中移除，将新检测到的特征线序列加入当前帧；

步骤2.3，优化相机位姿：在新图像的几何特征和上一幅图像进行关联之后，对步骤2.1中估计得到的相机位姿，进行优化，重投影得到的特征点p_t′应尽可能和匹配的特征点p_t重合，特征点的重投影误差采用两点的欧式距离度量；而特征线l_t′也应和匹配的特征线l_t重合，特征线的重投影误差采用端点到直线的距离进行度量；本方法融合点和线定义位姿优化的最小二乘问题：

其中，e_p和e_l分别为特征点和特征线的重投影误差，其计算公式为：

e_p＝p_t-π_p(p_t-1,T_t-1) (9)

式(10)中，

和

分别为直线l_t的两个端点的齐次坐标形式，从而计算出直线l_t的端点到重投影直线的距离之和；式(8)中，

为特征点的置信度，其表示为：

其中，n为该特征被检测到的次数，t₁为可调系数,Σ_p为该特征点的协方差矩阵，该矩阵和特征点所在ORB金字塔的级别直接相关；相应地，特征线的置信度

为：

其中，σ为特征线长度分布的方差值，t₂和t₃为可调系数，Σ_l为该特征线的协方差矩阵；迭代优化式(8)，直到误差收敛，所得结果即为当前帧的相机位姿；

步骤2.4，优化二维特征线：采用重投影的方式对线段进行优化矫正；首先，将每个特征线序列中的线段反投影到三维空间中：

得到三维线段L′_i,i表示某一个时刻；假设线段的端点服从高斯分布，分别统计两个端点的方差σ_L和均值然后依据σ_L和

对三维线段进行调整，

其中，α为优化系数，决定单次优化的力度；换言之，L′_i和均值的距离越大，优化力度越大；而端点分布的方差越大，优化力度越小；最后将优化后的线段投影到图像平面上完成对二维特征线的优化:

步骤2.5，判断是否设为关键帧,若为关键帧，则将其加入地图，否则返回步骤1；

步骤2.5.1，插入关键帧之后已处理多于10帧图像，且当前局部建图模块处于空闲状态，则判断为关键帧；

步骤2.5.2，上一次插入关键帧之后已处理超过20帧图像，则判断为关键帧；

步骤2.5.3，跟踪丢失并重定位后，已处理图像不得少于20帧，否则不能作为关键帧；

步骤2.5.4，当前图像上匹配的特征点不少于30个，且匹配的特征线不少于5条，否则不能作为关键帧；

步骤2.5.5，当前图像上跟踪的特征相比最近关键帧上跟踪的特征少于90％，则判断为关键帧；

若当前帧判断为关键帧，则生成关键帧并传递给局部建图模块以进行后续计算；当前跟踪模块返回步骤1，继续处理下一幅图像；

步骤3，局部建图：

步骤3.1，剔除冗余特征并插入新特征信息：对于新插入的关键帧，首先更新共视图和生成树，加入新关键帧节点；然后对局部地图中的三维特征进行裁剪和更新，由于被观测次数太少的特征有极大的可能是外点，所以裁剪掉三维地图中被观测次数少于三次的特征点和特征线，最后将该关键帧中匹配的新特征，使用三角化方法恢复特征深度信息，生成新三维特征并插入到地图中；

步骤3.2，优化局部地图：完整地图更新之后，对当前关键帧和与之关联的关键帧进行联合优化，主要优化对象为相关关键帧的相机位姿以及三维特征坐标；优化过程中所采用的重投影误差表达式和式(8)相同；在特征匹配时，不可避免地会引入一些外点，因此，在迭代优化10次后，将重投影误差过大的外点移除，再继续迭代优化，如此重复4次，以尽可能地清除外点，并最大程度地优化相机位姿；

步骤3.3，裁剪冗余关键帧：将关键帧之间特征的重合度大于90％的关键帧剔除，以精简共视图；

步骤4，回环检测：

步骤4.1，检测是否形成回环：首先，计算当前关键帧和相邻关键帧之间的词袋相似度，记相似度最小值为s_min，然后在运行时所维护的关键帧数据库中查询，若不存在相似度小于s_min的关键帧，则返回处理下一个关键帧；反之，则视为检测到回环；

步骤4.2，闭合回环：使用随机采样一致性算法计算两个关键帧之间的相似变换，然后将相似变换应用到当前关键帧与相邻关键帧上，完成闭环融合；

步骤4.3，优化全局地图，消除累积误差：对所有关键帧和三维特征进行优化，以消除累积误差，优化过程中，重投影误差的计算方式采用公式(8)所示内容。

3.根据权利要求1所述的一种基于点线融合的视觉SLAM方法，其特征在于：系统数据采用全局地图、局部地图、共视图三个结构辅助记录；全局地图存储所有三维路标以及每一个关键帧对应的相机位姿和二维观测信息；局部地图是全局地图的子集，记录了全局地图中和当前关键帧相邻的关键帧所包含的信息；而共视图包含了关键帧之间的关联；本发明方法适用于单目或双目系统。

4.根据权利要求1所述的一种基于点线融合的视觉SLAM方法，其特征在于：对于未标定的相机，可以使用相机在不同的视角下对棋盘格拍摄多张图像，采用张正友标定法计算相机参数。

5.根据权利要求1所述的一种基于点线融合的视觉SLAM方法，其特征在于：本方法实验环境为Ubuntu系统，i7处理器，16GB内存。

Images