基于复杂动态场景的位姿估计方法、系统及存储介质

本发明属于机器人自主定位与导航领域，具体涉及一种基于复杂动态场景的位姿估计方法、系统及存储介质。

背景技术：

1、视觉里程计作为视觉slam(同时定位和地图构建)系统中一个重要的环节，其主要利用相机获取到的图像序列来计算相机位姿，从而估计出运动轨迹。视觉里程计已经广泛应用于自主机器人、增强现实、虚拟现实等领域。

2、现有的视觉里程计系统主要可以分为三类：1)基于几何的视觉里程计系统，2)基于深度学习的视觉里程计系统，以及3)基于语义的视觉里程计系统。其中，现有基于几何的视觉里程计系统和基于深度学习的视觉里程计系统由于算法的局限性都难以在动态场景中使用。相比之下，现有的基于语义的视觉里程计系统主要将基于几何的视觉里程计与语义分割模型相结合，从而利用语义分割识别潜在的动态物体，并根据语义标签将动态物体上的特征点剔除，不参与后续的位姿计算，从而在一定程度上保证了视觉里程计位姿估计的精度。因此，基于语义的视觉里程计系统对动态场景具有较高的鲁棒性，能够满足通常情况动态场景下位姿估计的需求。由于基于语义的视觉里程计系统在动态场景所展示的优异性能，现已成为面向动态场景的视觉里程计的主流方向。

3、现有的基于语义的视觉里程计系统在面向复杂动态场景时，难以实现高精度、高鲁棒性的相机位姿估计。其主要的原因有三个：

4、第一，过度依赖语义分割的结果来剔除动态特征点，没有深度结合几何信息所提供的真实运动信息。在一方面，由于语义分割泛化能力有限，仅能在数据集覆盖的范围内实现较为准确的分割。因此，在语义分割未识别出的动态物体的情况下，动态特征点仍然会被保留下来；另一方面，由于语义分割识别出的物体为潜在的动态物体。因此，导致部分高质量静态点被剔除。这两方面均会导致相机位姿估计精度下降。

5、第二，没有考虑到图像运动模糊对下游任务的影响，例如：特征点提取、语义分割、运动一致性检测等，而这些算法依赖高质量的图像来保证算法计算精度。最终，图像运动模糊影响了后续的位姿估计精度。

6、最后，现有的大多数基于语义的视觉里程计系统都采用了算法复杂度较高的语义分割模型，虽然这些模型能够保证语义分割的精度，但是严重降低了视觉里程计系统的并发性，从而导致视觉里程计算法难以实时运行。

7、因此，针对上述技术问题，有必要提供一种基于复杂动态场景的位姿估计方法、系统及存储介质。

技术实现思路

1、有鉴于此，本发明的目的在于提供一种基于复杂动态场景的位姿估计方法、系统及存储介质，以保证复杂动态场景中的位姿估计精度以及实时性。

2、为了实现上述目的，本发明一实施例提供的技术方案如下：

3、一种基于复杂动态场景的位姿估计方法，所述位姿估计方法包括以下步骤：

4、s1、提取输入图像中的特征点，并通过稀疏光流金字塔法或者对极几何法对物体的运动状态进行运动一致性检测，得到运动一致性检测结果；

5、s2、通过轻量化语义分割模型对输入图像进行实时语义分割，得到语义分割结果；

6、s3、根据语义分割结果对场景中的物体进行语义标签分类，并根据语义标签分类结果及对应的深度图像进行潜在动态物体检测，得到潜在动态物体检测结果；

7、s4、基于几何信息和语义信息自适应剔除动态点，所述几何信息包括运动一致性检测结果和潜在动态物体检测结果，所述语义信息包括语义分割结果；

8、s5、基于保留的特征点进行位姿估计。

9、一实施例中，所述步骤s1及s2前还包括：

10、将输入图像转换为rgb图像；和/或，

11、对输入图像进行预处理，预处理包括运动模糊恢复预处理、直方平衡预处理、gamma矫正预处理中的一种或多种。

12、一实施例中，所述步骤s1中：

13、特征点通过orb特征提取算法、sift特征提取算法或surf特征提取算法进行提取。

14、一实施例中，所述步骤s1中通过稀疏光流金字塔法对物体的运动状态进行运动一致性检测具体为：

15、对连续的两张图像分别划分多层金字塔模型；

16、对划分完成的图像采样稀疏的特征点；

17、根据特征点计算光流，从而得到所采样的特征点的运动信息；

18、所述步骤s1中通过对极几何法对物体的运动状态进行运动一致性检测具体为：

19、利用八点法计算相邻帧之间的基础矩阵f；

20、根据下式计算特征点到对应极线的距离：

21、

22、l＝[a，b，c]t＝fp1，

23、其中，d为某个特征点到对应极线的距离，p2＝[u2，v2，1]t为当前特征点，p1＝[u1，v1，1]t为p2在上一帧中对应的特征点，u和v分别为特征点在图像中的横纵坐标，l为上一帧中与当前特征点p2匹配的特征点p1在当前帧的极线；

24、判断d的大小，若d小于预设距离阈值，则认定该特征点属于静态物体，否则，认定该特征点属于动态物体。

25、一实施例中，所述步骤s2后还包括：

26、对语义分割结果进行语义膨胀处理，得到语义膨胀处理后的结果sout为：

27、

28、其中，sout为语义膨胀处理后的语义分割结果图像，is为输入的语义分割结果图像，s为卷积模板或卷积核。

29、一实施例中，所述步骤s3中，根据语义分割结果对场景中的物体进行语义标签分类具体为：

30、根据语义分割结果将场景中的物体划分为高置信度动态物体、低置信度动态物体、高置信度静态物体或低置信度静态物体；

31、所述步骤s3中，根据语义标签分类结果及对应的深度图像进行潜在动态物体检测具体为：

32、根据语义标签分类结果及对应的深度图像分别计算高置信度动态物体和低置信度动态物体之间的关系值tgs、及高置信度动态物体和低置信度静态物体之间的关系值tsg为：

33、tgs/tsg＝αldepth+(1-α)ldistance；

34、其中，α作为深度和距离信息的指标，ldepth表示深度差，即ldepth＝ldepthg-ldepths，其中，ldepthg为高置信度动态物体的平均深度值，ldepths为低置信度动态物体或者低置信度静态物体的平均深度值，ldistance为质心坐标之间的欧氏距离，其中，cg为高置信度动态物体的质心坐标，cs为低置信度动态物体或低置信度静态物体的质心坐标；

35、获取阈值ts为：

36、

37、其中，simage为语义分割结果的面积，为平均深度差，ldepthg_i为第i个高置信度动态物体的深度值，ldepths_j为第j个低置信度动态物体或者低置信度静态物体的深度值，n为高置信度动态物体的类别数量，m为低置信度动态物体或者低置信度静态物体的类别数量，为平均欧氏距离，ldistance_i为第i个高置信度动态物体对所有低置信度动态物体或者低置信度静态物体的距离之和，n为高置信度动态物体的类别数量，m为低置信度动态物体或者低置信度静态物体的类别数量；

38、比较关系值tgs、关系值tsg与阈值ts的大小，当tgs＜ts或tsg＜ts时，认定该特征点属于潜在运动物体。

39、一实施例中，所述步骤s4包括：

40、获取运动一致性检测结果、语义分割结果及潜在动态物体检测结果中所有特征点总量结果r1，

41、若r1＞0，则进行自适应剔除动态点，否则，直接输出步骤s1中提取的特征点；

42、自适应剔除动态点具体为：

43、若r1＞l，则根据运动一致性检测结果剔除相应动态特征点；

44、若m＜r1≤l，则根据运动一致性检测结果和语义分割结果剔除相应动态特征点；

45、若s＜r1≤m，则根据运动一致性检测结果、语义分割结果和潜在高动态物体检测结果剔除相应动态特征点；

46、若r1≤s，则根据r1中所有结果剔除相应动态特征点；

47、判断自适应剔除动态点后的结果r2与阈值t的大小，若r2＜t，则返回重新执行自适应剔除动态点，否则，输出自适应剔除动态点后的结果r2；

48、其中，l、m、s、t满足s＜m＜l＜t。

49、一实施例中，所述步骤s5具体为：

50、通过epnp算法将保留的特征点求解旋转运动r和平移运动t，得到位姿信息{r，t}。

51、本发明另一实施例提供的技术方案如下：

52、一种基于复杂动态场景的位姿估计系统，所述位姿估计系统包括：

53、运动一致性检测模块，用于提取输入图像中的特征点，并通过稀疏光流金字塔法或者对极几何法对物体的运动状态进行运动一致性检测，得到运动一致性检测结果；

54、语义分割模块，用于通过轻量化语义分割模型对输入图像进行实时语义分割，得到语义分割结果；

55、语义标签分类模块，用于根据语义分割结果对场景中的物体进行语义标签分类；

56、潜在动态物体检测模块，用于根据语义标签分类结果及对应的深度图像进行潜在动态物体检测，得到潜在动态物体检测结果；

57、自适应动态点剔除模块，用于基于几何信息和语义信息自适应剔除动态点，所述几何信息包括运动一致性检测结果和潜在动态物体检测结果，所述语义信息包括语义分割结果；

58、位姿估计模块，用于基于保留的特征点进行位姿估计。

59、本发明又一实施例提供的技术方案如下：

60、一种机器可读存储介质，其存储有可执行指令，当所述指令被执行时使得所述机器执行上述的位姿估计方法。

61、本发明具有以下有益效果：

62、本发明首先针对复杂动态场景的存在的问题提出了针对性的视觉里程计框架；其次，充分考虑了语义分割对基于语义的视觉里程计算法的实时性的重要程度，因此，采用了轻量化的语义分割模型，来保证算法的并发性和实时性；最后，考虑到场景的复杂性，提出了基于多路几何-语义信息的自适应动态点剔除算法来保证位姿估计的精度。