[发明专利]一种基于单目视觉SLAM初始化方法

权利要求书

1.一种基于单目视觉SLAM初始化方法，其特征在于：方法包括如下步骤：

(1)从当前输入的图像中获取当前帧，对获取的所述当前帧进行关键帧筛选，并将所述当前帧送入帧列队；

(2)依次获取所述帧队列中每一关键帧，对获取的所述每一关键帧进行点线特征提取，并将所述点线特征作为种子点送入深度滤波器；

(3)对于每两关键帧，利用匹配获得的特征匹配对集合计算两关键帧之间的相对位姿，并处理获得点特征和线特征在空间中对应的三维点和三维直线在参考帧坐标系下的三维坐标，所有关键帧和三维点、三维直线构成初始地图；

(4)深度滤波器遍历种子点，在所述帧队列中每一帧上对其进行极线搜索、深度滤波，估计所述点线特征深度，并建立新的地图信息，完成单目视觉SLAM初始化过程。

2.根据权利要求1所述的一种基于单目视觉SLAM初始化方法，其特征在于：

对从图像中获取的当前帧进行关键帧筛选前还包括如下步骤：对处于初始状态的所述当前帧进行SLAM初始化；对处于正常状态的后续输入帧进行运动估计，通过稀疏图像对齐、特征细化、位姿及结构优化对所述当前帧进行运动估计。

3.根据权利要求1所述的一种基于单目视觉SLAM初始化方法，其特征在于：对关键帧进行点线特征提取中的特征包括角点特征、梯度点特征、梯度线特征；所述角点特征通过FAST算法提取，所述梯度线特征通过LSD算法提取。

4.根据权利要求1所述的一种基于单目视觉SLAM初始化方法，其特征在于：所述的角点特征和梯度线特征采用以下相同方式进行特征匹配：

两关键帧之间的一个线特征匹配对中，第一个关键帧的图像检测出的线特征的两个端点通过旋转矩阵R和平移矩阵t分别转换成第二个关键帧的图像中的两条极线，极线的方向向量采用如下公式计算：

其中，[t]×表示平移矩阵t的反对称矩阵，R为旋转向量，和为第一个关键帧的图像上检测出的线特征的两个端点的齐次坐标，l′_s和l′_e为第二个关键帧的图像上的两条极线的方向向量；

两条极线经过第二个关键帧的图像中的对极点，通过方向向量和对极点在图像中确定两条极线的位置，对极点为两关键帧的相机光心连线经过关键帧的图像所在成像平面的交点。

5.根据权利要求4所述的一种基于单目视觉SLAM初始化方法，其特征在于：所述的利用求解出的两关键帧之间的位姿，重构出点特征和线特征的三维坐标地具体过程为：对于一个点特征匹配对，已知第一个关键帧中的图像点像素齐次坐标和第二个关键帧中的图像点像素齐次坐标以及两个关键帧之间的旋转矩阵R和平移矩阵t，通过三角化得到关于这个点特征匹配对在空间中对应的三维点在第一个关键帧坐标系下的深度S₁的约束：

其中，为的反对称矩阵；

求解上述超定方程组，求得最小二乘解S₁，再通过以下公式求出三维点在第一个关键帧坐标系下的三维坐标P₁：

其中，K为相机的内参矩阵；

接着利用第一个关键帧和参考帧之间的旋转矩阵R和平移矩阵t，将三维点的坐标转换成参考帧坐标系的坐标。

6.根据权利要求5所述的一种基于单目视觉SLAM初始化方法，其特征在于：两关键帧之间的一个点特征匹配对中，一对点特征匹配对分为分别在两关键帧的图像中匹配的图像点P₁和图像点P₂，图像点P₁和图像点P₂在空间中对应的同一三维点的对极约束由以下公式建立：

S₁P₁＝KP

S₂P₂＝K(RP+t)

K^-1S₂P₂＝RK^-1S₁P₁+t

式中，P₁为三维点在两关键帧的第一个关键帧的图像中的图像点像素坐标，P₂为三维点在两关键帧的第二个关键帧的图像中的图像点像素坐标，K为相机的内参矩阵，P为三维点的三维坐标，R为两关键帧之间的旋转矩阵，t为两关键帧之间的平移矩阵，S₁为三维点在两关键帧的第一个关键帧中的深度，S₂为三维点在两关键帧的第二个关键帧中的深度，E为本质矩阵，T表示矩阵转置。