一种面向动态场景的双目视觉SLAM方法、电子设备和存储介质

本发明涉及一种面向动态场景的双目视觉slam方法、电子设备和存储介质。

背景技术：

1、同时定位与绘图(simultaneous localization and mapping,slam)是机器人领域的一项技术，旨在同时对机器人进行定位并构建环境地图。slam通常可以依赖于多个传感器，如摄像头、激光雷达和惯性测量单元，来收集有关环境的信息。如果相机是slam系统使用的主要传感器，则将其归类为视觉slam(v-slam)。

2、典型的视觉slam系统主要分为前端和后端：前端接受相机的数据，通过计算得到两帧图像之间相机的位姿变化(位姿即相机的运动中的位置和姿态)。前端的计算方法主要可以分为两种，光流直接法和特征点法。特征点可以理解成图像中有特点的点，包含关键点和描述子两种信息，其中描述子用于关键点的匹配。特征点法借助特征点的匹配来计算相机的运动位姿。而光流直接法不需要特征点，基于灰度不变假设，即同一个空间点的像素灰度值，在各个图像中是固定不变的，通过逐像素的计算来恢复相机运动。后端接受前端传过来的位姿数据，从一系列带噪声的位姿数据中估计最优的轨迹与地图。

3、视觉slam系统假设场景在系统运行期间保持静态，并通过观察静态物体来估计相机的自我运动。如果相机视场中出现动态物体并跟踪动态特征点，将严重影响姿态估计的精度，可能导致轨迹偏离或系统崩溃。现有很多技术来解决此问题，但仍有一些局限性。

4、传统方法中：例如光流方法，这种方法依赖于光流检测，对光照变化和遮挡高度敏感，这使得在复杂环境下对动态物体的处理难以执行；又如纯几何方法，这种方法受到特征点匹配的影响，对移动物体的感知力较弱，无法精准地去除动态物体以得到可靠的相机运动位姿；较新的深度学习方法，比如物体检测和语义分割，这些方法可以消除动态物体，但是基于预设的分类来消除特征点可能会排除静态对象的一些信息，导致系统的有效信息获取不足，估计的相机位姿不准确。

技术实现思路

1、本发明的目的在于克服现有技术的不足，提供一种面向动态场景的双目视觉slam方法、电子设备和存储介质。

2、本发明的目的是通过以下技术方案来实现的：

3、本发明的第一方面，提供一种面向动态场景的双目视觉slam方法，针对从双目相机获取的当前帧图像和先前帧图像，所述方法包括以下步骤：

4、获取所述当前帧图像和所述先前帧图像；

5、分别提取当前帧图像的静态特征点和动态特征点、先前帧图像的静态特征点和动态特征点；

6、利用静态特征点进行相机跟踪即位姿初始化，得到当前帧图像和先前帧图像的位姿；

7、利用当前帧图像和先前帧图像的位姿、当前帧图像和先前帧图像的动态特征点、已建立的地图点，将地图点与动态特征点进行匹配并进一步将当前图像出现的物体和先前帧图像出现的物体建立联系，实现连续跟踪的关联物体；

8、利用地图点与动态特征点的匹配结果，进行运动一致性检查确定目标的真实运动状态，输出当前帧图像中所有物体的运动状态；

9、根据当前帧图像中所有物体的运动状态，进行特征点的重过滤；

10、基于更新特征点后的当前帧图像进行定位和建图。

11、进一步地，所述实例分割模块采用fastinst网络模型，先验的动态内容包括人和车辆。

12、进一步地，所述分别提取当前帧图像的静态特征点和动态特征点、先前帧图像的静态特征点和动态特征点，方式相同，包括：

13、将待处理图像分别传入实例分割模块和特征点提取模块，所述实例分割模块分割任何先验的动态内容得到语义掩码，所述特征点提取模块从待处理图像中提取orb特征点；

14、基于所述语义掩码对orb特征点进行过滤，输出静态特征点和动态特征点。

15、进一步地，所述基于所述语义掩码对orb特征点进行过滤，输出静态特征点和动态特征点，采用基于四叉树的节点分裂的方式，包括以下步骤：

16、s11：确定初始的节点数目；

17、s12：根节点第一次分裂，根据节点分裂算法得到一个静态节点集合和一个动态节点集合；

18、s13：如果节点总数大于等于n则分裂结束，n为节点总数期望值，非极大值抑制单独应用于每个节点，否则进行s14；非极大值抑制是指在节点中选取响应值最高的一个特征点，一个节点可能包含很多特征点；特征点的响应值是为特征点和相邻特征点像素的差值；

19、s14：从节点集合中依次取节点，如果节点全部取完则进行s13，否则进行s15；

20、s15：根据节点分裂算法将取得的节点分裂，删除原节点，将分裂的节点分别加到静态节点集合和动态节点集合；

21、s16：如果节点总数<＝n，将存储在动态节点中的静态特征点提取并组合成新的静态节点，进行s7，否则直接进行s17；

22、s17：如果静态节点数量>2/n，进行s13，否则进行s18；

23、s18：如果静态节点的数量仍然不足，则拆分所有静态节点，直到它们的数量超过了n/2或者进一步拆分变得不可行，然后进行s13；

24、所述节点分裂算法包括以下子步骤：

25、sa1：节点1次分裂，分裂为4个节点；分裂之后根据图像的尺寸划分节点的区域，设置4个节点为静态节点；

26、sa2：将特征点依次加入对应范围的节点，判断特征点是否全部加入节点，如果全部加入，则将4个节点分别放入静态节点集合和动态节点集合，完成操作；否则进行sa3；

27、sa3：根据掩码判断特征点是否是掩码内的点，如果是则置此特征点所在的节点为动态节点，否则为静态节点；重复sa3。

28、进一步地，所述利用当前帧图像和先前帧图像的位姿、当前帧图像和先前帧图像的动态特征点、已建立的地图点，将地图点与动态特征点进行匹配并进一步将当前图像出现的物体和先前帧图像出现的物体建立联系，实现连续跟踪的关联物体，包括：

29、s21：定义在当前帧图像中的物体对象集合为ocf，ocf＝{x1,x2,…,xi,…,xm}，在先前帧图像中的物体对象集合为opf，opf＝opf＝{y1,y1,…,yj,…,yn}，其中xi、yj，分别代表一个对象；通过特征匹配和三维投影，将当前帧图像的每个对象xi与前一帧图像的对象关联起来，将对象xi与对象yj的匹配权值表示为wij；

30、s22：当前帧图像的xi中包含的特征点pl与前一帧图像中相同对象的映射中对应点的匹配分两个阶段求值：首先计算描述符匹配距离，如果从地图点pk生成的最小描述符距离dlpk小于给定的阈值thdes，则计算重投影误差，由下面的公式计算：

31、

32、其中是地图pk在世界坐标系下的三维坐标点，sk代表其深度；t为低成本跟踪估计的当前帧的姿态值即所述位姿，k为摄像机的内部参数，ul为特征点pl的像素坐标；

33、s23：如果重投影误差errlk小于给定的阈值thproj，更新wij为wij+2，其中j代表地图点pk属于opf的第j个物体；否则尝试通过描述符将特征点pl与yj∈opf包含的特征点进行匹配，进行s24；

34、s24：如果由pq产生的最小描述符距离小于给定的阈值thdes，更新wij为wij+1，其中j代表地图点p属于opf的第j个物体；wij的更新机制总结为下面公式：

35、

36、进一步地，所述利用当前帧图像和先前帧图像的位姿、当前帧图像和先前帧图像的动态特征点、已建立的地图点，将地图点与动态特征点进行匹配并进一步将当前图像出现的物体和先前帧图像出现的物体建立联系，实现连续跟踪的关联物体，还包括：

37、s25：为观察噪声会导致当前帧中的对象xi和前一帧中的多个对象具有匹配关系，为了确保唯一和最佳匹配，将两帧之间的对象匹配作为一个加权二部图匹配问题，并使用km算法求解；

38、s26：输出匹配权值大于设置阈值的匹配。

39、进一步地，所述利用地图点与动态特征点的匹配结果，进行运动一致性检查确定目标的真实运动状态，输出当前帧图像中所有物体的运动状态，包括：

40、s31：检查两个对象之间的每个匹配点对(pl,pq)的运动状态；其中pl表示对象xi上的一个特征点，pq表示对象yj上对应的一个特征点；当当前帧图像中的对象xi和先前帧图像中的对象yj匹配时，执行此检查；

41、s32：如果一个地图点是通过pl构建的即当前帧图像的点，那么它被投影到fl中；否则，pl的临时地图点由三角测量构造；四扒鸡三角测量指的是，通过在两处观测同一个空间点的夹角，从而确定该点的距离和深度；只有知道了空间点的深度才能进行s33；完成s32，进行s33；

42、s33：根据对象关联中的重投影误差公式计算errlk；如果投影误差小于预设阈值，将匹配点对分类为静态匹配，否则它被认为是动态匹配；

43、s34：如果xi和yj之间的匹配点对存在多个动态匹配，则认为xi处于运动状态。

44、进一步地，所述根据当前帧图像中所有物体的运动状态，进行特征点的重过滤，包括：

45、在xi处于运动状态这种情况下，在后续的姿态估计过程中丢弃与对象xi相关的所有特征点。

46、本发明的第二方面，提供一种面向动态场景的双目视觉slam电子设备，包括存储单元和处理单元，所述存储单元上存储有可在所述处理单元上运行的计算机指令，所述处理单元运行所述计算机指令时执行第一方面所述的一种面向动态场景的双目视觉slam方法的步骤。

47、本发明的第三方面，提供一种存储介质，其上存储有计算机指令，所述计算机指令运行时执行第一方面所述的一种面向动态场景的双目视觉slam方法的步骤。

48、本发明的有益效果是：

49、在本发明的一示例性实施例中，针对动态和临时静态对象进行设计，结合了深度学习方法和纯几何方法，当相机捕捉到连续移动的物体和突然运动的物体时，能够快速有效地过滤出真正的动态特征点；引入了一种有效的目标运动状态检测方法来检测动态目标，从而抑制了动态目标对姿态估计的影响。该方法充分利用了相机获得的有效信息，能够实现在动态场景下的相机位姿的准确估计和地图建立，具有较强的鲁棒性和稳定性。