[发明专利]基于线特征的SLAM系统快速重定位算法

权利要求书

1.一种基于线特征的SLAM系统快速重定位算法，其特征在于，包括以下步骤：

步骤一、分线程将候选关键帧与当前图像帧进行点线特征匹配；基于点线匹配数量判断点线初步匹配数量是否要求达到设定数量，若满足则执行步骤二；

步骤二、设置随机一致性算法参数，并执行EPnL算法估计初步位姿；

步骤三、基于初步估计的位姿，剔除步骤一中点特征误匹配的点特征对；

步骤四、执行点线特征匹配同时使用的位姿优化估计，得到内点和内线数量；

步骤五、根据优化后的有效内点、内线数目，分情况讨论是否执行投影匹配，完成最终重定位；

所述的步骤二中，EPnL算法步骤如下：

1)假设世界坐标系下的控制点为基于输入的n个地图线中点世界坐标对其进行计算；

2)通过世界坐标系下的控制点和地图线中点世界坐标计算α_ij参数，计算公式如下：

3)输入线特征的M_L矩阵：

假定当前相机的标定矩阵为：

其中，fx、fy为图像焦距，cx、cy为光心偏移。

2.根据权利要求1所述的基于线特征的SLAM系统快速重定位算法，其特征在于，所述的步骤一中，是基于DBOW2库训练的字典对ORB点特征进行匹配，基于FLANN算法对LSD-LBD线特征进行匹配。

3.根据权利要求1所述的基于线特征的SLAM系统快速重定位算法，其特征在于，所述的步骤三具体是指在使用线特征完成位姿估计后，需要对匹配点特征进行基于重投影误差的后验判断，进一步剔除误差较大或者误匹配的点特征对。

4.根据权利要求3所述的基于线特征的SLAM系统快速重定位算法，其特征在于，点特征重投影误差ErrorPoint2的计算公式如下：

ErrorPoint2＝||x-K(RP+t)||² (7)

其中，x为实际观测到的点特征图像坐标，K为3*3标定矩阵，P为地图点的世界坐标，R为3*3的旋转矩阵，t为3*1的平移矩阵。

5.根据权利要求1所述的基于线特征的SLAM系统快速重定位算法，其特征在于，所述的步骤四具体是指位姿优化结束后，需要基于新的位姿，对匹配的点线特征进行误差检测，剔除误差较大的匹配特征对，记录内点和内线数量。

6.根据权利要求1所述的基于线特征的SLAM系统快速重定位算法，其特征在于，所述的步骤五分以下情况：

1)若优化后不满足最终重定位条件，可以分为以下三种情况：

a.若点线特征在优化后均不满足最终重定位条件，则基于当前图像帧位姿，执行分线程的投影匹配，增大点线特征匹配数量；

b.若只有点特征在优化后不满足最终重定位条件，则基于当前图像帧位姿，执行点特征的投影匹配，增大点特征匹配数量；

c.若只有线特征在优化后不满足最终重定位条件，则基于当前图像帧位姿，执行线特征的投影匹配，增大线特征匹配数量；

在匹配结束后，若重新满足重定位条件，则执行第二次点线特征位姿优化，并执行类似于第一次优化后的判断；

2)若优化后满足最终重定位条件，则直接退出，重定位成功。