[发明专利]一种基于光流跟踪和双几何模型的初始位姿估计方法

权利要求书

1.一种基于光流跟踪和双几何模型的初始位姿估计方法，其特征在于：包括以下步骤：

S10，通过检测图像的FAST关键点，选取第一帧图像；

S20，使用光流跟踪法对第一帧图像的FAST关键点进行匹配，并对匹配的FAST关键点进行视觉差检测，归一化所有关键点；

S30，创建两个线程，并行地计算单应矩阵和基础矩阵模型；

其中，计算单应矩阵和基础矩阵时，采用RANSC算法迭代地进行以下子步骤：

S31，随机选取求解模型用的8对匹配关键点；

S32，分别求解单应矩阵和基础矩阵；

S33，计算匹配关键点在此次迭代求解的单应矩阵和基础矩阵上的对称转换误差；

S34，将满足对称转换误差阈值的关键点作为内点，利用内点的对称转换误差计算此次迭代求解的模型的得分；

S35，更新模型最高得分SH、SF，并保存最高得分所对应的模型和内点；

其中模型得分的计算公式如下：

在上述计算公式中：

S_M为模型M的得分；

分别为第1张图像的特征点通过模型M转换到第2张图像上的对称转换误差、第2张图像的特征点通过模型M转换到第1张图像上的对称转换误差；t_M为内点的对称转换误差阈值；

Γ为与单应矩阵的对称转换误差阈值t_H相等；

S40，根据单应矩阵和基础矩阵模型得分选择合适的模型进行求解，得到多组初始位姿，利用每组位姿对内点进行三角化估计特征点深度，并计算3D点的重投影误差和3D点在两个图像帧中的视差角，用于选择最优的初始位姿，如果多组位姿解里面存在最优解，则初始化成功，否则初始化失败；

其中，单应矩阵或基础矩阵模型选择机制如下：

如果R_H大于0.45则选择单应矩阵模型求解初始位姿，否则选取基础矩阵模型求解初始位姿。

2.如权利要求1所述的一种基于光流跟踪和双几何模型的初始位姿估计方法，其特征在于：步骤S10中，当FAST关键点数量满足阈值时，选取该帧图像为第一帧图像。

3.如权利要求1所述的一种基于光流跟踪和双几何模型的初始位姿估计方法，其特征在于：

步骤S20中包括以下子步骤：

S21，使用光流跟踪法对第一帧图像的FAST关键点进行匹配，当匹配数量达到阈值时，对匹配的FAST关键点进行视觉差检测；

S22，计算所有跟踪到的FAST关键点的像素移动距离的中值，如果移动距离中值大于距离阈值，则归一化所有关键点。