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

摘要

本发明公开一种基于光流跟踪和双几何模型的初始位姿估计方法，包括以下步骤：通过检测图像的FAST关键点，选取第一帧图像；使用光流跟踪法对第一帧图像的FAST关键点进行匹配，并对匹配的FAST关键点进行视觉差检测，归一化所有关键点；创建两个线程，并行地计算单应矩阵和基础矩阵模型；根据单应矩阵和基础矩阵模型得分选择合适的模型进行求解。该发明只对第一帧图像帧检测FAST关键点，然后使用光流跟踪法进行关键点匹配，可以实现快速的数据关联；同时估计单应矩阵模型和基础矩阵模型，并通过打分机制选择合适的模型求解初始位姿，可以在平面和非平面场景中鲁棒地进行单目视觉里程计的初始化，并且估计的初始姿态的误差较小。

主权项

1.一种基于光流跟踪和双几何模型的初始位姿估计方法，其特征在于：包括以下步骤：S10，通过检测图像的FAST关键点，选取第一帧图像；S20，使用光流跟踪法对第一帧图像的FAST关键点进行匹配，并对匹配的FAST关键点进行视觉差检测，归一化所有关键点；S30，创建两个线程，并行地计算单应矩阵和基础矩阵模型；其中，计算单应矩阵和基础矩阵时，采用RANSC算法迭代地进行以下子步骤：S31，随机选取求解模型用的8对匹配关键点；S32，分别求解单应矩阵和基础矩阵；S33，计算匹配关键点在此次迭代求解的单应矩阵和基础矩阵上的对称转换误差；S34，将满足对称转换误差阈值的关键点作为内点，利用内点的对称转换误差计算此次迭代求解的模型的得分；S35，更新模型最高得分SH、SF，并保存最高得分所对应的模型和内点；其中模型得分的计算公式如下：在上述计算公式中：SM为模型M的得分；分别为第1张图像的特征点通过模型M转换到第2张图像上的对称转换误差、第2张图像的特征点通过模型M转换到第1张图像上的对称转换误差；t_M为内点的对称转换误差阈值；Γ为与单应矩阵的对称转换误差阈值tH相等；S40，根据单应矩阵和基础矩阵模型得分选择合适的模型进行求解，得到多组初始位姿，利用每组位姿对内点进行三角化估计特征点深度，并计算3D点的重投影误差和3D点在两个图像帧中的视差角，用于选择最优的初始位姿，如果多组位姿解里面存在最优解，则初始化成功，否则初始化失败；其中，单应矩阵或基础矩阵模型选择机制如下：如果RH大于0.45则选择单应矩阵模型求解初始位姿，否则选取基础矩阵模型求解初始位姿。