CN109934862A - 一种点线特征结合的双目视觉slam方法 - Google Patents

Abstract

本发明公开了一种点线特征结合的双目视觉SLAM方法，包括以下步骤：S1，双目相机内参标定；S2，利用标定后的相机采集环境图像，利用梯度密度滤波器过滤特征密集区域，得到有效的图像检测区域；S3，提取特征点和特征线；S4，对提取出的线特征进行断线合并；S5，利用特征点线进行跟踪匹配，并选取关键帧；S6，利用特征点线的重投影误差构建代价函数；S7，进行局部地图优化；S8，利用点线联合的词袋模型判断闭环，并对全局轨迹进行优化。本发明提出一种图像滤波机制、线段合并方法及加速的后端优化计算方法，解决了图像特征密集区域提取大量无效特征、线特征提取方法存在断线及传统后端优化方法耗时长等问题，提高系统的鲁棒性、精度及速度。

Description

一种点线特征结合的双目视觉SLAM方法

技术领域

本发明属于摄影测量和计算机视觉技术领域，尤其涉及一种点线特征结合的双目视觉SLAM方法。

背景技术

以点特征为主的SLAM算法能够实时的进行特征跟踪、构图、闭环检测，完成同步定位与地图构建，是目前视觉领域最为主流的算法之一。但点特征容易受到光照、噪声的干扰，且构建出的三维点地图比较稀疏，无法表达真实的场景结构。尤其是在一些低纹理场景下，经常会出现特征不足，算法性能下降的情况。与点特征相比，线特征承载了更多的结构信息，引入线特征的点线融合算法可以更好地还原场景结构，即使在纹理不充足的场景下依然表现良好。然而传统基于LSD的线特征提取方法易造成线段过分割等问题，使算法精度下降。且在大规模三维重建中，传统的计算方法效率低下，不适于实时计算。

发明内容

为了解决上述问题，能够更加鲁棒、高效、准确地完成相机定位与地图构建，本发明提出一种点线特征结合的双目视觉SLAM方法，可以提高算法鲁棒性，速度及精度。

为达到上述目的，本发明所采用的技术方案是：

一种点线特征结合的双目视觉SLAM方法，包括以下步骤：

步骤1，进行相机内参标定，所述相机内参包括相机主点、焦距与畸变系数；

步骤2，将移动平台上的双目相机采集的视频影像处理成一系列图像帧序列，并利用图像滤波算法滤除图像中特征过于稠密的区域，得到有效的检测区域；

步骤3，在步骤2中得到的有效检测区域上进行特征点和结构线的提取；

步骤4，根据获取的结构线，进行断线合并操作；

步骤5，根据步骤3获取的特征点和步骤4中合并后的结构线进行跟踪匹配，并选取关键帧；

步骤6，利用特征点和结构线的重投影误差构建代价函数；

步骤7，对步骤6中代价函数进行求解以优化局部地图；

步骤8，判断相机运动轨迹是否形成闭环，获取正确的闭环关键帧，对全局的相机姿态和地图进行整体优化；

步骤9，输出结果。

进一步地，所述步骤2的实现方式如下：首先对获取的图像帧序列计算图像中像素点的梯度τ_ij，将梯度值高于给定阈值G_s的像素点标记为1，其余标记为0；然后，以标记为1的像素点为中心，在5×5像素的区域内统计标记为1的像素点数占该区域所有像素数的百分比记为ρ_ij，若ρ_ij>ρ_DENSE＝0.75，则认定该区域为稠密区域，继续扩展该区域，将21×21的像素区域过滤掉，不进行线段检测；最后，重复进行以上步骤，将局部稠密区域过滤掉，最终输出有效的检测区域。

进一步地，所述步骤4的实现方法如下：首先根据角度贴近性，水平距离接近度和垂直距离接近度原则进行线段分组；然后进一步考虑线长和间距的影响，根据更精细划分的空间接近度和角度接近度得出最终合并后的线段，最后重复迭代地执行线段分组与线段合并步骤直到无法再合并为止。

进一步地，所述步骤6的实现方法如下：首先利用局部地图中关键帧点线的特征匹配信息与3D点线投影到2D图像平面上的投影点线信息计算重投影误差，为避免错误匹配带来的影响引入Huber鲁棒核函数构建最终的代价函数。

进一步地，所述步骤7的实现方法如下：利用LM算法计算步骤6中的代价函数，根据H矩阵的稀疏性进行特征分块，并使用LDLT分解方法加快增量方程的计算，最终求出增量方程的位姿与路标增量，以调整空间点线的位置和相机位姿。

进一步地，所述步骤8的实现方式如下：首先对视觉关键帧提取得到的点、线特征描述子，建立图像数据库；然后根据建立的视觉词典，把图像的特征转换成词包向量，利用L1范数距离计算图像间相似度得分；最后根据相似度得分来判断两幅图之间是否形成了闭环，若发生闭环则进行全局优化以调整相机姿态和路标点的位置。

由于采用本发明的技术方案，其有益效果为：

本发明在基于点特征SLAM算法的基础上引入了线特征，可以更好地还原场景结构，即使在纹理不充足的场景下依然表现良好。在特征提取前期，提出了图像滤波的预处理方法，利用梯度密度滤波器将图像中特征密集区域剔除以降低后期特征匹配的计算复杂度提高匹配精度。同时在LSD算法的基础上提出了线段合并方法，将部分断线进行合并，更进一步地提升了系统的精度。在局部地图优化阶段，利用H矩阵的稀疏性，提出一种快速矩阵运算方法，提高了系统运行效率。

附图说明

图1为本发明方法实施例的流程图。

图2为本发明进行线段合并操作的流程图。

图3为本发明中点投影误差示意图。

图4为本发明中线投影误差示意图。

图5为本发明中H矩阵的稀疏结构。

图6为本发明基于聚类方法建立的综合点线特征的视觉词典模型

图7为本发明方法实施例所建的点线特征地图。

具体实施方式

下面将结合附图以及具体实施方式对本发明作进一步描述。

如图1所示，一种点线特征结合的双目视觉SLAM方法，主要包括以下步骤：

步骤1，利用相机获取多张不同视角下固定大小的棋盘格影像数据；采用张正友相机标定法，对获取到的棋盘格影像数据进行相机内参计算，获取相机标定结果。

步骤2，利用图像滤波器滤除图像中特征密集区域，具体操作子步骤如下，

步骤2.1，对视频中获取的图像帧序列计算图像中像素点的梯度τ_ij，根据像素梯度强度阈值G_s，将梯度值高于阈值G_s的像素点标记为1，低于阈值G_s的标记为0；以标记为1的像素点为中心，在5×5像素的区域内统计标记为1的像素点数占该区域所有像素数的百分比记为ρ_ij；

步骤2.2，若ρ_ij>ρ_DENSE＝0.75，认定该区域为稠密区域，则继续扩展该区域，将21×21的像素区域过滤掉，不进行线段检测；重复步骤2.1及步骤2.2，将局部稠密区域过滤掉，最终输出有效的检测区域。

步骤3，在步骤2中得到的滤波处理后的图像序列上进行特征点和结构线的提取，具体操作方法如下，

步骤3.1，对获取的图像序列建立金字塔，分别在金字塔图像上进行影像分块，获取一定窗口大小的影像区域，利用ORB特征点提取算法在每个分块里分别进行特征点提取和描述；

步骤3.2，对图像帧序列进行高斯滤波，利用LSD算子进行结构线提取；

步骤3.3，针对提取了特征点和结构线后的序列帧进行分块，分别将特征点和结构线划分到不同的图像区域中。

步骤4，根据获取的结构线，进行断线合并操作。具体操作流程如图2所示，主要包括以下子步骤：

步骤4.1，按照提取出的结构线长度进行排序，选出其中最长的线段L₁；然后利用角度贴近性，水平距离接近度和垂直距离接近度原则找出与线段L₁在角度，水平距离和垂直距离方向上都比较接近的候选线段组P_L1；

步骤4.2，根据两线段最近端点距离d_ij筛选出两条线段空间接近程度小于阈值d_s的候选线段组然后考虑线段长度和间距的影响因素，对候选线段组中的角度阈值做出修正，具体修正方法如下，

(1)令当前较长线段为L₁，线段l_i∈{L₂，L₃，…，L_s}的阈值修正系数为λ_i为：

其中，为两条线段间距的加权系数，0<ξ_s<1，用以调整线段长度和间距的比例关系，而且d_s＝ξ_s×l₁。

(2)综合考虑线段长度和两条线段间距的影响，线段L_i的自适应角度阈值为：

若两线段间的夹角则初步满足合并标准，将其预合并为线段M；若预合并后的线段M与较长线段L₁之间的绝对角度差小于合并成立，以合并线段M替换L₁和L_i，否则，放弃合并。最后重复线段分组及合并这两个步骤，直到无法再合并为止。

步骤5，根据步骤3获取的特征点和步骤4中合并后的结构线，进行特征点和结构线跟踪匹配，并选取关键帧送入局部地图，具体包含如下步骤，

步骤5.1，根据步骤3中获取的特征点，利用特征描述符距离进行特征点跟踪，获取较好的跟踪特征点对；

步骤5.2，根据步骤4中获取的结构线，利用结构线的参数进行结构线跟踪，获取较理想的跟踪结构线对；

步骤5.3，利用当前跟踪的特征点对和结构线对，对当前的影像姿态进行初步优化；

步骤5.4，采用一种融合视觉内容变化与时间的关键帧选择方法，选取有价值的关键帧插入局部地图，具体有如下几条要求，

1.距离上一次关键帧的插入已经过了20帧或者局部地图构建线程空闲；

2.当前帧至少跟踪到了50个特征点和15条空间直线；

3.当前帧包含参考关键帧中的特征要少于75％。

步骤6，利用特征点和结构线的重投影误差构建代价函数，具体构建方法如下，

步骤6.1，如图3所示为点投影误差求解的示意图，主要分为以下三步：首先将世界坐标系下的3D点P_w转到相机坐标系中，然后根据投影模型将相机坐标系下的3D点P_c投影到图像上，得到2D投影点P_uv′，最后计算投影点p_uv′与匹配特征点p_uv的误差：

其中P_w，i是第i个像素点在世界坐标系下的3D坐标，T_cw，k表示第k帧图像从世界坐标系转到相机坐标系下，K表示相机内参，P_uv，k，i表示第k帧中的像素点i在图像坐标系下的表示。

步骤6.2，如图4所示为线投影误差求解的示意图，主要分为以下三步：首先将普吕克坐标表示的空间直线L_w转到相机坐标系中，然后根据相机模型，计算空间直线L_c在图像上的投影直线L_uv′，最后观测线段两端点P_s、P_e到投影直线L_uv′的距离：

其中L_w，j是第j条空间线在世界坐标系下的3D坐标，T_cw，k为第k帧图像从世界坐标系转到相机坐标系下，K表示相机内参，L_uv，k，j表示第k帧中的像素点j在图像坐标系下的表示。

步骤6.3，为避免错误匹配对精度的影响，引入Huber核函数，其具体形式如下，

其中e为包含了点与线的总误差，当e的值大于阈值δ时，目标函数的增长由二次函数转为一次函数，从而抑制了错误匹配对的影响，使整个优化求解更为稳定。

步骤6.4，构建综合点线的投影误差的代价函数：

其中Ω_P和Ω_l分别表示点、线的协方差矩阵，H_P和H_l分别表示点、线的Huber鲁棒核函数。

步骤7，对步骤6.4中代价函数进行求解以优化局部地图，具体包括如下子步骤；

步骤7.1，利用LM算法表示步骤6.4中构建的总代价函数：

(J^TJ+λdiag(J^TJ))Δx＝-J^Tf(x)

将左边的系数(J^TJ+λdiag(J^TJ))定义为H，右边定义为g，得到增量方程的简化形式：

HΔx＝g

步骤7.2，根据H矩阵的稀疏性将矩阵分为U、V、W和W^T四个部分，如图5所示，利用高斯消元法消去路标对相机位姿的影响，求得关于相机位姿的增量Δ_Ci：

(U-WV^-1W^T)Δ_Ci＝r_Ci-WV^-1r_mi

再根据相机位姿计算路标点线的位置Δ_mi：

Δ_mi＝V^-1(r_mi-W^TΔ_Ci)

步骤7.3，在求解(U-WV^-1W^T)项时，采用LDLT方法(一种改进的平方根分解法)加速矩阵的求解，最终可以求得相机的位姿增量Δ_Ci，然后再通过相机位姿增量求得空间路标点线的空间位姿Δ_mi，从而优化局部地图。

步骤8，利用点线联合的词袋模型判断闭环检测，具体包括如下子步骤，

步骤8.1，从使用大量的点特征描述子中进行训练得到图像数据库，然后将点线特征进行分类，用聚类方法均值聚类方法建立综合点线特征的离线视觉词典，如图6所示，用于在线过程中识别和查询相似的图像进行回环检测，在建立词典的过程中通过增加标志位的方法使得点线特征在视觉词典里和建立图像数据库时区别对待；

步骤8.2，根据提取到的特征把图像转换成词包向量，词包向量VPL_i既包含了点特征词包VP_i又包含了线特征词包VL_i；

步骤8.3，计算两帧图像间的相似度得分：

在进行图像相似度判断时，可直接比较图像词包向量间的相似度得分，同时为了避免将相似度较高的邻近帧误判为闭环，设置了阈值k，只有当j>i+k时，才进行闭环检测。

步骤9，输出由点线特征构建的环境地图详见图7所示，这是由点和线构成的稀疏地图，由图7可见，加入线特征后，场景的结构信息更加突出，可以清晰的辨别，地面，墙面，墙角等信息。

Claims (6)

1.一种点线特征结合的双目视觉SLAM方法，其特征在于，包括以下步骤：

步骤1，进行相机内参标定，所述相机内参包括相机主点、焦距与畸变系数；

步骤2，将移动平台上的双目相机采集的视频影像处理成一系列图像帧序列，并利用图像滤波算法滤除图像中特征过于稠密的区域，得到有效的检测区域；

步骤3，在步骤2中得到的有效检测区域上进行特征点和结构线的提取；

步骤4，根据获取的结构线，进行断线合并操作；

步骤5，根据步骤3获取的特征点和步骤4中合并后的结构线进行跟踪匹配，并选取关键帧；

步骤6，利用特征点和结构线的重投影误差构建代价函数；

步骤7，对步骤6中代价函数进行求解以优化局部地图；

步骤8，判断相机运动轨迹是否形成闭环，获取正确的闭环关键帧，对全局的相机姿态和地图进行整体优化；

步骤9，输出结果。

2.根据权利要求1所述的一种点线特征结合的双目视觉SLAM方法，其特征在于：所述步骤2的实现方式如下：首先对获取的图像帧序列计算图像中像素点的梯度τ_ij，将梯度值高于给定阈值G_s的像素点标记为1，其余标记为0；然后以标记为1的像素点为中心，在5×5像素的区域内统计标记为1的像素点数占该区域所有像素数的百分比记为ρ_ij，若ρ_ij>ρ_DENSE＝0.75，则认定该区域为稠密区域，继续扩展该区域，将21×21的像素区域过滤掉，不进行线段检测；最后，重复进行以上步骤，将局部稠密区域过滤掉，最终输出有效的检测区域。

3.根据权利要求1所述的一种点线特征结合的双目视觉SLAM方法，其特征在于：所述步骤4的实现方法如下：首先根据角度贴近性，水平距离接近度和垂直距离接近度原则进行线段分组；然后进一步考虑线长和间距的影响，根据更精细划分的空间接近度和角度接近度得出最终合并后的线段，最后重复迭代地执行线段分组与线段合并步骤直到无法再合并为止。

4.根据权利要求1所述的一种点线特征结合的双目视觉SLAM方法，其特征在于：所述步骤6的实现方法如下：首先利用局部地图中关键帧点线的特征匹配信息与3D点线投影到2D图像平面上的投影点线信息计算重投影误差，为避免错误匹配带来的影响引入Huber鲁棒核函数构建最终的代价函数。

5.根据权利要求1所述的一种点线特征结合的双目视觉SLAM方法，其特征在于：所述步骤7的实现方法如下：利用LM算法计算步骤6中的代价函数，根据H矩阵的稀疏性进行特征分块，并使用LDLT分解方法加快增量方程的计算，最终求出增量方程的位姿与路标增量，以调整空间点线的位置和相机位姿。

6.根据权利要求1所述的一种点线特征结合的双目视觉SLAM方法，其特征在于：所述步骤8的实现方式如下：首先对视觉关键帧提取得到的点、线特征描述子，建立图像数据库；然后根据建立的视觉词典，把图像的特征转换成词包向量，利用L1范数距离计算图像间相似度得分；最后根据相似度得分来判断两幅图之间是否形成了闭环，若发生闭环则进行全局优化以调整相机姿态和路标点的位置。