本发明涉及信息技术领域，尤其涉及一种基于光流跟踪和双几何模型的初始位姿估计方法。
背景技术：
在单目视觉里程计的开始阶段，只有图像间的信息，没有三维空间信息，此时需要进行初始化，估计初始位姿和构建初始地图。现有的初始位姿估计方法主要有光流跟踪+单应矩阵估计方法和orb+单应矩阵/基础矩阵估计方法。光流跟踪+单应矩阵估计方法通过光流跟踪fast关键点，进行数据关联，使用单应矩阵模型来求解初始位姿；orb+单应矩阵/基础矩阵估计方法通过orb特征的提取与匹配进行数据关联，然后并行计算单应矩阵和基础矩阵，选择合适的模型来求解初始位姿。
但是，上述两种方法都存在一定的缺陷。光流跟踪+单应矩阵初始位姿估计方法存在一个强假设：图像特征点处于同一平面上，大多数情况下的场景并不满足该假设，特征点并不在同一平面上，因此在这种情况下求解的初始位姿误差较大，容易导致单目视觉里程计初始化失败，鲁棒性较差。orb+单应矩阵/基础矩阵初始位姿估计方法，需要对每帧图像提取特征和计算特征描述子，计算量较大；另外没有在选择第二帧图像时先对特征点的像素视差进行检查，在像机平移较小的情况下，完成模型的求解后，选择最优初始位姿时容易出现不存在最优初始位姿，而导致前面的模型求解无效。
技术实现要素：
鉴于以上内容，本发明旨在提供一种基于光流跟踪和双几何模型的初始位姿估计方法基于光流跟踪和双几何模型的初始位姿估计方法，采用光流跟踪和双几何模型对初始位姿进行估计，完成单目视觉里程计的初始化，具有较好的鲁棒性、较高的模型平均得分以及较小的姿态估计误差。
为达到上述目的，本发明所采用的技术方案是：一种基于光流跟踪和双几何模型的初始位姿估计方法，包括以下步骤：
s10，通过检测图像的fast关键点，选取第一帧图像；
s20，使用光流跟踪法对第一帧图像的fast关键点进行匹配，并对匹配的fast关键点进行视觉差检测，归一化所有关键点；
s30，创建两个线程，并行地计算单应矩阵和基础矩阵模型；
其中，计算单应矩阵和基础矩阵时，采用ransc算法迭代地进行以下子步骤：
s31，随机选取求解模型用的8对匹配关键点；
s32，分别求解单应矩阵和基础矩阵；
s33，计算匹配关键点在此次迭代求解的单应矩阵和基础矩阵上的对称转换误差；
s34，将满足对称转换误差阈值的关键点作为内点，利用内点的对称转换误差计算此次迭代求解的模型的得分；
s35，更新模型最高得分sh、sf，并保存最高得分所对应的模型和内点；
其中模型得分的计算公式如下：
在上述计算公式中：
sm为模型m的得分；
γ为与单应矩阵的对称转换误差阈值th相等；
s40，根据单应矩阵和基础矩阵模型得分选择合适的模型进行求解，得到多组初始位姿，利用每组位姿对内点进行三角化估计特征点深度，并计算3d点的重投影误差和3d点在两个图像帧中的视差角，用于选择最优的初始位姿，如果多组位姿解里面存在最优解，则初始化成功，否则初始化失败；
其中，单应矩阵或基础矩阵模型选择机制如下：
如果rh大于0.45则选择单应矩阵模型求解初始位姿，否则选取基础矩阵模型求解初始位姿。
进一步地，步骤s10中，当fast关键点数量满足阈值时，选取该帧图像为第一帧图像。
进一步地，步骤s20中包括以下子步骤：
s21，使用光流跟踪法对第一帧图像的fast关键点进行匹配，当匹配数量达到阈值时，对匹配的fast关键点进行视觉差检测；
s22，计算所有跟踪到的fast关键点的像素移动距离的中值，如果移动距离中值大于距离阈值，则归一化所有关键点。
相对现有技术，本发明具有以下有益效果：
该发明采用光流跟踪和双几何模型对初始位姿进行估计，完成单目视觉里程计的初始化，只对第一帧图像帧检测fast关键点，然后使用光流跟踪法进行关键点匹配，可以实现快速的数据关联；同时估计单应矩阵模型和基础矩阵模型，并通过打分机制选择合适的模型求解初始位姿，可以在平面和非平面场景中鲁棒地进行单目视觉里程计的初始化，并且估计的初始姿态的误差较小。
【附图说明】
图1是本发明一种基于光流跟踪和双几何模型的初始位姿估计方法的一流程图。
图2是本发明步骤s2的子流程图。
图3是本发明步骤s3的子流程图。
如下具体实施方式将结合上述附图进一步说明本发明。
【具体实施方式】
为了本领域技术人员更加清楚地理解本发明技术方案，以下结合说明书附图对本发明技术方案作进一步详细说明，值得注意的是，以下具体实施方式仅用以说明本发明技术方案，而非限定。
请参阅图1、图2及图3，在本发明的一种基于光流跟踪和双几何模型的初始位姿估计方法，包括以下步骤：
s10，通过检测图像的fast关键点，选取第一帧图像；当fast关键点数量满足阈值时，则选取该帧图像为第一帧图像；
s20，使用光流跟踪法对第一帧图像的fast关键点进行匹配，并对匹配的fast关键点进行视觉差检测，归一化所有关键点；
其中，包括以下子步骤：
s21，使用光流跟踪法对第一帧图像的fast关键点进行匹配，当匹配数量达到阈值时，对匹配的fast关键点进行视觉差检测；
s22，计算所有跟踪到的fast关键点的像素移动距离的中值，如果移动距离中值大于距离阈值，则归一化所有关键点。
s30，创建两个线程，并行地计算单应矩阵和基础矩阵模型；
其中，计算单应矩阵和基础矩阵时，采用ransc算法迭代地进行以下子步骤：
s31，随机选取求解模型用的8对匹配关键点；
s32，分别求解单应矩阵和基础矩阵；
s33，计算匹配关键点在此次迭代求解的单应矩阵和基础矩阵上的对称转换误差；
s34，将满足对称转换误差阈值的关键点作为内点，利用内点的对称转换误差计算此次迭代求解的模型的得分；
s35，更新模型最高得分sh、sf，并保存最高得分所对应的模型和内点；
其中模型得分的计算公式如下：
在上述计算公式中：
sm为模型m的得分；
г为与单应矩阵的对称转换误差阈值th相等；
s40，根据单应矩阵和基础矩阵模型得分选择合适的模型进行求解，得到多组初始位姿，利用每组位姿对内点进行三角化估计特征点深度，并计算3d点的重投影误差和3d点在两个图像帧中的视差角，用于选择最优的初始位姿，如果多组位姿解里面存在最优解，则初始化成功，否则初始化失败；
其中，单应矩阵或基础矩阵模型选择机制如下：
如果rh大于0.45则选择单应矩阵模型求解初始位姿，否则选取基础矩阵模型求解初始位姿。
试验例
从公开标准数据集theeurocmavdataset的machinehall01中的选取8个初始化场景，对使用光流跟踪+单应矩阵估计方法、orb+单应矩阵/基础矩阵估计方法以及本发明的光流跟踪+单应矩阵/基础矩阵估计方法，三种初始位姿估计方法进行单目视觉里程计的初始化试验对比。为了有效对比三种初始位姿估计方法，试验中使用相同的初始图像帧作为输入，以及统一使用orb+单应矩阵/基础矩阵估计方法中单目视觉里程计初始化结果判断条件和模型评分机制对求解的模型进行打分。
对比指标主要有：单目视觉里程计初始化结果以及初始化所需的处理时间、模型平均得分、姿态估计误差。其中：
1.初始化结果：初始化成功与否；即从模型中求解的多组初始位姿中，如果存在最优解，则初始化成功，用1表示；否则初始化失败，用0表示。
2.处理时间：包括从特征提取、特征匹配、模型计算、位姿估计等所有处理时间。
3.模型平均得分：利用满足对称转换误差阈值的内点对模型进行打分，模型的得分除以内点数量即为模型平均得分。用于评价求解的单应矩阵或基础矩阵的质量。
4.姿态估计误差：由于单目视觉里程计初始化求解的初始位姿具有尺度不确定性，实验中仅对比了求解的初始姿态的估计误差。以旋转向量的形式表示姿态，将真实旋转向量与估计的初始姿态的旋转向量相减的模作为评价指标。
使用三种初始位姿估计方法的单目视觉里程计初始化在8个初始化场景下的实验对比结果如表1所示。表1中，lk_h表示光流跟踪+单应矩阵估计方法，orb_h_f表示orb+单应矩阵/基础矩阵估计方法，lk_h_f表示本发明的光流跟踪+单应矩阵/基础矩阵估计方法。
表1实验对比结果
从表1的结果可以看出，该发明的方法具有较好的鲁棒性、较高的模型平均得分以及较小的姿态估计误差。
上述说明是针对本发明较佳可行实施例的详细说明，但实施例并非用以限定本发明的专利申请范围，凡本发明所提示的技术精神下所完成的同等变化或修饰变更，均应属于本发明所涵盖专利范围。