[发明专利]点线特征视觉SLAM系统的快速位姿估计算法

说明书

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

技术领域

本发明属于计算机视觉领域，尤其涉及一种点线特征视觉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线长度倍数范围内的线特征进行剔除。