[发明专利]一种基于sparse-slam的实时建图方法

权利要求书

1.一种基于sparse-slam的实时建图方法，其特征在于，包括如下两个并列运行的线程：

线程1，跟踪并提取关键帧，具体步骤为：

步骤1.1，对输入的初始帧构建N层的图像金字塔，并提取初始帧图像金字塔中的各个特征点，设置包含初始帧图像金字塔以及各个特征点的初始帧KF1；

步骤1.2，对新输入的当前帧构建N层的图像金字塔，并提取当前帧图像金字塔中的各个特征点，再利用SSD算法在当前帧中跟踪初始帧KF1中的各个特征点，SSD算法公式为：

D＝∑((I_ki-u_k)-(I_ci-u_c))²

式中，I_ki表示初始帧KF1中第i个特征点的像素值，I_ci表示当前帧中第i个特征点的像素值，u_k和u_c分别表示初始帧KF1中第i个特征点周围图像块的像素均值以及当前帧中第i个特征点周围图像块的像素均值；

步骤1.3，若步骤1.2中SSD算法跟踪成功跟踪到的特征点数量小于M，则返回步骤1.1重新设置初始帧KF1，若成功跟踪到的特征点数量大于M，则将当前帧标记为关键帧KF2；

步骤1.4，根据各个成功跟踪到的特征点，利用相机对极几何模型估算出相机的姿态R和T，R表示关键帧KF2与初始帧KF1之间的旋转矩阵，T表示关键帧KF2与初始帧KF1之间的平移矩阵，将初始帧KF1和关键帧KF2加入到关键帧队列KFS中；

步骤1.5，在点云MAP初始化成功后，将点云MAP映射到当前帧中，并在当前帧中利用SSD算法找出与其对应的二维点，从而利用相机对极几何模型估算出当前相机的姿态R和T，并将当前帧标记为KFi，加入到KFS中；

线程2，创建并插入点云MAP，具体步骤为：

步骤2.1，判断是否创建坐标系，若已创建，转到步骤2.2，否则创建坐标系，即根据关键帧队列KFS中的初始帧KF1建立世界坐标系W，X轴和Y轴方向为初始帧KF1的长和宽方向，Z轴方向为相机指向初始帧KF1的方向；

步骤2.2，遍历关键帧队列KFS中每一个未三角化的KFi，i＝2，3，4...，利用对极几何原理将KFi中的二维特征点三角化为三维特征点，再将三维特征点加入到点云MAP中。

2.根据权利要求1所述的基于sparse-slam的实时建图方法，其特征在于，利用相机对极几何模型估算出相机的姿态R和T的估算公式为：

P_l＝R(P_r-T)

(R^TP_r)^T·T×P_l＝0

式中，P_l和P_r为二幅图像中匹配上的特征点，R^T为R的转置，×为叉乘。

3.根据权利要求1所述的基于sparse-slam的实时建图方法，其特征在于，对输入的初始帧构建N层的图像金字塔的具体步骤为：对每一层图像进行下采样得到上一层的图像，通过N次下采样便可得到N层的图像金字塔。

4.根据权利要求1所述的基于sparse-slam的实时建图方法，其特征在于，提取初始帧图像金字塔中的各个特征点的具体步骤为：对每一层图像用FAST算法提取图像的角点，将角点作为特征点。