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

权利要求书

1.一种点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，包括以下步骤：

步骤一、分线程点线特征提取及特征的网格分配，将线特征根据中点位置分配到预先划定的图像帧网格中；

步骤二、通过上一帧跟踪的地图线中点，对地图点及地图线中点分线程投影匹配，并将线特征误匹配的线特征进行剔除；

步骤三、对剔除线特征误匹配后的地图点及地图线中点进行位姿优化算法；

步骤二中，线特征匹配流程如下：

1)将上一帧跟踪上的地图线中点，通过估计的位姿矩阵T投影至当前图像帧中；

2)基于预设的半径、线特征图像金字塔提取层数，获取步骤一中包含网格的所有线特征；

3)进行LBD描述子比较，并类比于点特征匹配计算各个线特征的旋转方向，存于直方图；

4)根据后验线特征的旋转一致性，对于不属于主要旋转角角度的线特征匹配进行剔除；

5)后验长度一致性，对于当前图像帧线特征长度不在地图线平均2D线长度倍数范围内的线特征进行剔除；

步骤三具体步骤包括：

1)基于纯点特征的先行位姿优化估计；并执行多次优化迭代；

2)基于点线特征的同时位姿优化估计，使用上步1)中确认为内点和内线的匹配对进行位姿优化；

所述的点线特征的重投影误差E_Both表示为：

其中，E_Lines为线重投影误差之和，E_Points为点重投影误差之和，e_p,k为当前匹配到的第k个地图点的重投影误差，e_l,j为当前匹配到的第j个地图线的重投影误差；Np为当前图像帧匹配到的地图点总数，Nl为当前图像帧匹配到的地图线总数，Ω_p,k为当前点特征的协方差矩阵，Ω_l,j为当前线特征的协方差矩阵；

优化使用的线特征中点的雅克比矩阵公式如下：

其中，l_j＝[l_x,j,l_y,j,l_z,j]^T为当前图像帧第j个测量线特征的方向向量；P_mid,j＝[X_mid,j,Y_mid,j,Z_mid,j]^T为当前图像帧与第j个测量线特征匹配的世界坐标系下3D线中点坐标；fx、fy为图像焦距，I为单位矩阵，Λ表示有向量到矩阵的变换，ξ为李代数。

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

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

4.根据权利要求1所述的点线特征视觉SLAM系统的快速位姿估计算法，其特征在于，所述的长度倍数为0.6-1.4。