点线特征视觉SLAM系统的快速位姿估计算法技术方案

本发明专利技术公开了一种点线特征视觉SLAM系统的快速位姿估计算法，包括以下步骤：步骤一、分线程点线特征提取及特征的网格分配，将线特征根据中点位置分配到预先划定的图像帧网格中；步骤二、地图点及地图线中点的分线程投影匹配，对线特征误匹配的线特征进行剔除；步骤三、基于步骤二进行地图点及地图线中点的位姿优化算法。本发明专利技术所提出的位姿估计算法是点线特征SLAM系统正常工作时使用的算法，通过线特征中点信息的引入，对本来复杂的4自由度线特征进行简化处理，使点线特征位姿估计问题的效率得到极大地提升。

A fast pose estimation algorithm for point line feature vision slam system

The invention discloses a fast pose estimation algorithm of a point line feature vision slam system, which comprises the following steps: Step 1: extract the point line feature of the split line process and assign the feature grid, assign the line feature to the pre delimited image frame grid according to the mid point position; step 2: Match the split line projection of the map point and the mid point of the map line, and carry out the line feature of the line feature mismatch The third step is based on the second step to optimize the position and orientation of map points and map lines. The pose estimation algorithm proposed by the invention is the algorithm used in the normal operation of the point line feature slam system. By introducing the midpoint information of the line feature, the original complex 4-DOF line feature is simplified, so that the efficiency of the pose estimation problem of the point line feature is greatly improved.

【技术实现步骤摘要】
点线特征视觉SLAM系统的快速位姿估计算法
本专利技术属于计算机视觉领域，尤其涉及一种点线特征视觉SLAM系统的快速位姿估计算法。
技术介绍
同时定位与地图构建(SLAM)，随着无人驾驶、无人机技术的发展变得越发重要。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自己的运动。若以相机为传感器，就称为“视觉SLAM”。目前基于特征法SLAM系统，以纯点特征居多。但是从处理低纹理场景和实现系统的可视化角度，纯点特征明显无法满足需求，然而线特征的引入能够有效解决上述两个问题。另一方面，线特征的引入具有增强视觉SLAM系统的鲁棒性和可视性的优点，但是与点特征相比，线特征具有四个自由度(两个端点各有一对x,y坐标)，无疑极大提高了系统的计算量。视觉SLAM的前端定位线程，需要实时性要求，如何对获得的线特征进行处理，进而实现更为快速有效的位姿估计是极为重要的问题。SLAM系统的位姿估计，主要涉及三个大步骤：特征提取、特征匹配和位姿优化。在点线特征SLAM系统中，线特征提取占用了大量时间，因此特征匹配和位姿优化的效率变得尤为关键。目前唯一开源的参考文献，在线特征匹配时采用快速最邻近搜索函数库(FLANN)，位姿优化时采用匹配点特征+线特征端点的估计方式。实际上，如果对提取到的线特征信息进行预处理，或是将计算量分到其他线程，将有效提高前端的跟踪效率。
技术实现思路
本专利技术要解决的技术问题是提供一种可以加速处理线特征，实现SLAM系统前端线特征快速匹配和优化，完成点线特征位姿估计的算法。本专利技术的技术方案如下：一种点线特征视觉SLAM系统的快速位姿估计算法，包括以下步骤：步骤一、分线程点线特征提取及特征的网格分配，将线特征根据中点位置分配到预先划定的图像帧网格中；步骤二、通过上一帧跟踪上的地图线中点，对地图点及地图线中点的分线程投影匹配，并将线特征误匹配的线特征进行剔除；步骤三、对剔除线特征误匹配后的地图点及地图线中点进行位姿优化算法。作为本专利技术的进一步改进，步骤一中，使用ORB作为点特征算法，LSD-LBD作为线特征算法；在分线程完成点线特征提取后，参照点特征的网格分配方式，将线特征根据中点位置分配到预先划定的图像帧网格中。作为本专利技术的进一步改进，取线特征单元网格为点特征单元网格的2-4倍。作为本专利技术的进一步改进，步骤二中，线特征匹配流程如下：1)将上一帧跟踪上的地图线中点，通过估计的位姿矩阵T投影至当前图像帧中；2)基于预设的半径、线特征图像金字塔提取层数，获取步骤一中包含网格的所有线特征；3)进行LBD描述子比较，并类比于点特征匹配计算各个线特征的旋转方向，存于直方图；4)根据后验线特征的旋转一致性，对于不属于主要旋转角角度的线特征匹配进行剔除；5)后验长度一致性，对于当前图像帧线特征长度不在地图线平均2D线长度倍数范围内的线特征进行剔除。作为本专利技术的进一步改进，所述的长度倍数为0.6-1.4。作为本专利技术的进一步改进，步骤三具体步骤包括：1)基于纯点特征的先行位姿优化估计；并执行多次优化迭代；2)基于点线特征的同时位姿优化估计，使用上步1)中确认为内点和内线的匹配对进行位姿优化。作为本专利技术的进一步改进，所述的点线特征的重投影误差EBoth表示为：其中，ELines为线重投影误差之和，EPoints为点重投影误差之和，ep,k为当前匹配到的第k个地图点的重投影误差，el,j为当前匹配到的第j个地图线的重投影误差；Np为当前图像帧匹配到的地图点总数，Nl为当前图像帧匹配到的地图线总数，Ωp,k为当前点特征的协方差矩阵，Ωl,j为当前线特征的协方差矩阵；优化使用的线特征中点的雅克比矩阵公式如下：其中，lj＝[lx,j,ly,j,lz,j]T为当前图像帧第j个测量线特征的方向向量；Pmid,j＝[Xmid,j,Ymid,j,Zmid,j]T为当前图像帧与第j个测量线特征匹配的世界坐标系下3D线中点坐标；fx、fy为图像焦距，I为单位矩阵，Λ表示有向量到矩阵的变换。与现有技术相比，本专利技术的有益效果为：本专利技术所提出的位姿估计算法是点线特征SLAM系统正常工作时使用的算法，通过线特征中点信息的引入，对本来复杂的4自由度线特征进行简化处理，使点线特征位姿估计问题的效率得到极大地提升。步骤一通过对线特征中点进行预分配分配，可以在避免在步骤二匹配过程中传统使用线端点进行匹配的二义性，并且基于线中点进行匹配计算量更小，因此计算小路更高。步骤三通过使用固定地图线中点进行位姿优化，可以在保证线信息充分利用的前提下，将计算量减半。另外，通过与目前位姿估计速度最快的进行对比，可以看出在10次运行中，本专利技术提出的位姿估计算法的平均时间，均提升接近1倍。附图说明图1为本专利技术算法的流程示意图；图2为PL-SLAM正常运行时的2D图像；图3为PL-SLAM正常工作时的Pangolin地图；图4为ubuntu终端下本专利技术主要步骤的时间执行记录。具体实施方式为了更好地理解本专利技术，下面结合附图通过具体的实施例来具体说明本专利技术的技术方案。本专利技术提出了基于线特征中点的网格预匹配方法和点特征+地图线中点的加速优化算法。基于线特征中点的匹配方法，可以避免使用线特征端点的网格预匹配的复杂性，而地图线中点作为地图线端点的“融合信息”参与位姿估计，可以在保证信息充分使用的前提下，将线特征参与位姿估计的计算量直接减半。本专利技术适用的对象为视觉SLAM中的点线SLAM系统。在完成分线程点线特征提取，并基于点特征和线特征中点位置进行网格分配后，参考ORB-SLAM2框架中点特征的投影匹配，设计地图线中点的投影匹配方法。点线特征匹配后，基于地图点和地图线中点，执行需要数据量更少的位姿优化。同时，为提高优化收敛速度，在执行点线特征同时位姿优化前，先执行一次仅基于点特征的位姿优化估计。最后基于ORB-SLAM2软件框架扩展的PL-SLAM前端运动模型跟踪为例，验证本专利技术提出方法的有效性。如图1所示，本专利技术的实施主要包括以下三个步骤：步骤一、分线程点线特征提取及特征的网格分配本专利技术使用ORB作为点特征算法，LSD-LBD作为线特征算法。在分线程完成点线特征提取后，参照点特征的网格分配方式，将线特征根据中点位置分配到预先划定的图像帧网格中。注：线特征使用的图像帧网格较点特征网格应该略大，因为实际场景下，线特征存在因截断、遮挡等问题导致的线特征中点位置偏移，无法像点特征一样更为精准定位。默认取线特征单元网格为点特征单元网格的2～4倍。步骤二、地图点及地图线中点的分线程投影匹配本专利技术的点特征匹配方法未做变动。线特征匹配流程如下：(1)将上一帧跟踪上的地图线中点，通过估计的位姿矩阵T投影至当前图像帧中。(本文档来自技高网...

【技术保护点】
1.一种点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，包括以下步骤：/n步骤一、分线程点线特征提取及特征的网格分配，将线特征根据中点位置分配到预先划定的图像帧网格中；/n步骤二、通过上一帧跟踪上的地图线中点，对地图点及地图线中点的分线程投影匹配，并将线特征误匹配的线特征进行剔除；/n步骤三、对剔除线特征误匹配后的地图点及地图线中点进行位姿优化算法。/n

【技术特征摘要】
1.一种点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，包括以下步骤：
步骤一、分线程点线特征提取及特征的网格分配，将线特征根据中点位置分配到预先划定的图像帧网格中；
步骤二、通过上一帧跟踪上的地图线中点，对地图点及地图线中点的分线程投影匹配，并将线特征误匹配的线特征进行剔除；
步骤三、对剔除线特征误匹配后的地图点及地图线中点进行位姿优化算法。


2.根据权利要求1所述的点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，步骤一中，使用ORB作为点特征算法，LSD-LBD作为线特征算法；在分线程完成点线特征提取后，参照点特征的网格分配方式，将线特征根据中点位置分配到预先划定的图像帧网格中。


3.根据权利要求2所述的点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，取线特征单元网格为点特征单元网格的2-4倍。


4.根据权利要求1所述的点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，步骤二中，线特征匹配流程如下：
1)将上一帧跟踪上的地图线中点，通过估计的位姿矩阵T投影至当前图像帧中；
2)基于预设的半径、线特征图像金字塔提取层数，获取步骤一中包含网格的所有线特征；
3)进行LBD描述子比较，并类比于点特征匹配计算各个线特征的旋转方向，存于直方图；
4)根据后验线特征的旋转一致性，对于不属于主要旋转角角度的线特征匹配进行剔除；
5)后验长度一致性，对于当前图像帧线特征长度不在地...

【专利技术属性】
技术研发人员：朱战霞，马廷宸，王铮，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西;61