阅读说明：本技术 动态环境下的视觉slam初始化方法、系统、装置 (Visual SLAM (simultaneous localization and mapping) initialization method, system and device in dynamic environment ) 是由 汤淑明 卢晓昀 顿海洋 黄馨 张力夫 于 2019-09-27 设计创作，主要内容包括：本发明属于机器人、无人驾驶、AR视觉领域,具体涉及一种动态环境下的视觉SLAM初始化方法、系统、装置,旨在解决SLAM在动态环境下缺乏提取静态特征点的问题。本系统方法包括获取有视差的两帧图像；获取前一帧图像的匹配特征点；前一帧图像等份划分,得到多个图像块,将包含匹配特征点大于设定阈值的图像块作为格子模型,并获取格子模型的内点和质心；通过内点计算格子模型的耦合度,基于预设的耦合度阈值选取格子模型构建对应的格子模型集合；基于质心计算每一个格子模型集合的分布方差,选取值最大的集合构建静态特征集合；将静态特征集合三角化,通过非线性优化对SLAM初始化。本发明能提取足够的静态特征点。(The invention belongs to the fields of robots, unmanned driving and AR vision, in particular relates to a visual SLAM initialization method, system and device in a dynamic environment, and aims to solve the problem that SLAM lacks of extracting static feature points in the dynamic environment. The system method comprises the steps of obtaining two frames of images with parallax; acquiring matching feature points of a previous frame of image; dividing the previous frame of image in equal parts to obtain a plurality of image blocks, taking the image blocks containing the matched characteristic points larger than a set threshold value as a lattice model, and obtaining the interior points and the mass center of the lattice model; calculating the coupling degree of the lattice model through the inner points, and selecting the lattice model based on a preset coupling degree threshold value to construct a corresponding lattice model set; calculating the distribution variance of each lattice model set based on the mass center, and selecting the set with the largest value to construct a static feature set; and triangulating the static feature set, and initializing the SLAM through nonlinear optimization. The invention can extract enough static characteristic points.)

动态环境下的视觉SLAM初始化方法、系统、装置

技术领域

本发明属于机器人、无人驾驶、AR视觉领域，具体涉及一种动态环境下的视觉SLAM初始化方法、系统、装置。

背景技术

视觉SLAM是一个系统工程，在经典的视觉SLAM框架中，往往会假设环境为静态环境，或者环境中的静态特征占据多数。尽管目前已经出现了较为成熟的视觉SLAM系统，如ORB-SLAM、DSO、LSD等，但由于SLAM系统做了静态环境的假设，导致其在运动场景中的鲁棒性极差。无论是直接法或是非直接法的视觉SLAM系统，都缺乏应对运动物体的干扰，而且在SLAM运行过程中，对于相机位姿的解算完全依赖于RANSAC方法，如果静态特征点在整个图像中未占据多数，或者占比较低，则在有限次的迭代计算中，往往得不到正确的相机运动模型。

传统的因式分解法由于需要提前预知运动物体的个数，且解算数据需要批量数据，因此难以实时且鲁棒的解算运动环境下的相机位姿。建模的方法如高斯建模，混合高斯建模等对于SLAM意义不大，原因在于SLAM的真正意义在于对未知环境的探索，如果在已知环境中运动，SLAM问题可退化为定位问题，我们完全可以限定环境以各种方式获取环境的高精地图，然后利用地图匹配对相机进行定位。

目前，越来越多基于深度学习的方法被应用在运动目标检测中。在某些视觉SLAM框架中，采用了对运动目标进行先识别，再对运动目标区域内的特征点进行剔除，最后利用剩余的特征点进行相机运动模型计算。基于深度学习的方法对运动目标的检测的准确率和效率有了较大的提升，但仍然具有极大的局限性，这是由于其识别信息往往依据目标的纹理、颜色、灰度等，这些信息与目标是否运动并无绝对关系。而其判别目标是否运动的依据往往是在有限的环境中找出可能运动的物体去除，这种方式的缺点非常明显，主要体现在两个方面：第一，可能会丢失大量本可以用于定位的静态特征点信息。例如，在室内环境中，由于人是具有运动属性的目标，该框架会将识别的所有的人所在区域内的特征点统统删除，而忽略了在某些情况下，人可能是静止的，当静止的人占据图像视野的大部分时，会丢失许多本可以用于定位的特征点信息。第二，在陌生环境下往往会有许多运动物体由于深度学习训练网络的训练数据的局限而无法被识别，换言之，即深度学习无法识别所有的运动物体，如此一来，深度学习神经网络的作用仅仅在于去除部分的、有限的运动物体或运动特征点，而未被识别的运动物体或运动的特征点依然会对视觉定位产生极大影响。

如何有效的获得外点率低的图像中的静态特征点(不符合静态特征的模型的点)，而非像深度学习利用纹理，颜色，灰度等来判定运动属性，对于解算相机位姿有着重要意义。因此，本发明提出了一种动态环境下的视觉SLAM初始化方法。

发明内容

为了解决现有技术中的上述问题，即为了解决现有视觉SLAM系统在动态环境下缺乏提取静态特征点，导致无法实时、鲁棒的解算SLAM系统中相机的位姿的问题，本发明第一方面，提出了一种动态环境下的视觉SLAM初始化方法，该方法包括：

步骤S10，从输入视频中获取具有视差的第一图像帧、第二图像帧；

步骤S20，对所述第一图像帧、所述第二图像帧，分别进行ORB特征点的提取，通过特征点匹配方法获取所述第一图像帧的匹配特征点；

步骤S30，对所述第一图像帧等份划分，得到多个图像块，选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，并通过RANSAC算法获取每个格子模型的内点，及内点的质心；

步骤S40，对每一个格子模型，基于其内点分别计算其与其他格子模型的耦合度，并基于预设的耦合度阈值选取格子模型，构建对应的格子模型集合；

步骤S50，对于每一个格子模型集合，基于其中每个格子模型内点的质心计算分布方差，并选取最大分布方差值对应的格子模型集合，并基于该集合中各格子模型的内点构建静态特征集合；

步骤S60，对所述静态特征集合中的内点进行三角化，通过非线性优化方法获取SLAM的三维坐标，并基于该三维坐标对SLAM进行初始化。

在一些优选的实施方式中，所述的第一图像帧为时刻在前的图像。

在一些优选的实施方式中，步骤S20中“通过特征点匹配方法获取所述第一图像帧的匹配特征点”，其方法为：获取所述第一图像帧与所述第二图像帧相匹配的ORB特征点作为所述第一图像帧的匹配特征点。

在一些优选的实施方式中，步骤S30中“选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，并通过RANSAC算法获取每个格子模型的内点，及内点的质心”，其方法为：

选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，统计格子模型的数量；

若所述格子模型的数量大于预设值，则通过RANSAC算法获取每个格子模型的内点，及内点的质心，执行步骤S40，否则执行步骤S10。

在一些优选的实施方式中，步骤S40中“对每一个格子模型，基于其内点分别计算其与其他格子模型的耦合度”，其方法为：

将每一个格子模型作为第一模型，将其他格子模型中的任一模型作为第二模型；

基于所述第二模型的内点，通过RANSAC算法筛选符合所述第一模型解算的内点，并统计符合所述第一模型解算的内点的数量；

将所述数量与所述第二模型的内点数量的比值作为所述第一模型与所述第二模型的耦合度。

在一些优选的实施方式中，步骤S50中“对于每一个格子模型集合，基于其中每个格子模型内点的质心计算分布方差”，其计算方法为：

其中，X_variance为每个格子模型内点的质心在x方向的方差，Y_variance为每个格子模型内点的质心在y方向的方差，F_distribution为分布方差。

本发明的第二方面，提出了一种动态环境下的视觉SLAM初始化系统，该系统包括获取模块、特征匹配模块、筛选模块、构建集合模块、计算分布方差模块、初始化模块；

所述的获取模块，配置为从输入视频中获取具有视差的第一图像帧、第二图像帧；

所述的特征匹配模块，配置为对所述第一图像帧、所述第二图像帧，分别进行ORB特征点的提取，通过特征点匹配方法获取所述第一图像帧的匹配特征点；

所述的筛选模块，配置为对所述第一图像帧等份划分，得到多个图像块，选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，并通过RANSAC算法获取每个格子模型的内点，及内点的质心；

所述的构建集合模块，配置为对每一个格子模型，基于其内点分别计算其与其他格子模型的耦合度，并基于预设的耦合度阈值选取格子模型，构建对应的格子模型集合；

所述的计算分布方差模块，配置为对于每一个格子模型集合，基于其中每个格子模型内点的质心计算分布方差，并选取最大分布方差值对应的格子模型集合，并基于该集合中各格子模型的内点构建静态特征集合；

所述的初始化模块，配置为对所述静态特征集合中的内点进行三角化，通过非线性优化方法获取SLAM的三维坐标，并基于该三维坐标对SLAM进行初始化。

本发明的第三方面，提出了一种存储装置，其中存储有多条程序，所述程序应用由处理器加载并执行以实现上述的动态环境下的视觉SLAM初始化方法。

本发明的第四方面，提出了一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；所述程序适用于由处理器加载并执行以实现上述的动态环境下的视觉SLAM初始化方法。

本发明的有益效果：

本发明能提取足够的静态特征点，实时且鲁棒的解算SLAM中相机的位姿。本发明提供了一种基于区域分割、全局耦合、分布度判断的动态环境下视觉SLAM初始化方法，利用静态特征点与运动物体在图像上分布的差别，筛选出静态特征点集合，保证提取足够的静态特征点。并根据静态特征点集合进行相机的位姿解算，之后对静态特征点进行三角化，完成对周围环境的初始化。以静态特征点集合的方式提供给SLAM系统静态语义信息，能够使系统鲁棒实时的在动态环境中运行。

附图说明

通过阅读参照以下附图所做的对非限制性实施例所做的详细描述，本申请的其他特征、目的和优点将会变得更明显。

图1是本发明一种实施例的动态环境下的视觉SLAM初始化方法的流程示意图；

图2是本发明一种实施例的动态环境下的视觉SLAM初始化系统的框架示意图；

图3是本发明一种实施例的动态环境下的视觉SLAM初始化方法的第一图像帧等份划分的示例图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明的动态环境下的视觉SLAM初始化方法，如图1所示，包括以下步骤：

步骤S10，从输入视频中获取具有视差的第一图像帧、第二图像帧；

步骤S20，对所述第一图像帧、所述第二图像帧，分别进行ORB特征点的提取，通过特征点匹配方法获取所述第一图像帧的匹配特征点；

步骤S30，对所述第一图像帧等份划分，得到多个图像块，选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，并通过RANSAC算法获取每个格子模型的内点，及内点的质心；

步骤S40，对每一个格子模型，基于其内点分别计算其与其他格子模型的耦合度，并基于预设的耦合度阈值选取格子模型，构建对应的格子模型集合；

步骤S50，对于每一个格子模型集合，基于其中每个格子模型内点的质心计算分布方差，并选取最大分布方差值对应的格子模型集合，并基于该集合中各格子模型的内点构建静态特征集合；

步骤S60，对所述静态特征集合中的内点进行三角化，通过非线性优化方法获取SLAM的三维坐标，并基于该三维坐标对SLAM进行初始化。

为了更清晰地对本发明动态环境下的视觉SLAM初始化方法进行说明，下面结合附图1对本发明方法一种实施例中各步骤进行展开详述。

步骤S10，从输入视频中获取具有视差的第一图像帧、第二图像帧。

SLAM(即时定位与地图构建)可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图。

在本实施例中，利用相机获取周围环境的视频流，基于视频流获取两帧之间具有时间间隔且具有视差的RGB图像，两帧图像按照时间顺序排列，分别记为第一图像帧、第二图像帧。

步骤S20，对所述第一图像帧、所述第二图像帧，分别进行ORB特征点的提取，通过特征点匹配方法获取所述第一图像帧的匹配特征点。

本实施例中，先提取步骤S10中获取的两帧RGB图像的ORB特征点，然后将第一帧和第二帧的RGB图像的特征点进行匹配。

图像特征的检测和提取是计算机视觉最重要的研究领域之一，ORB(OrientedFAST and Rotated BRIEF)算法是目前最快速稳定的特征点检测和提取算法，ORB特征是将FAST特征点的检测方法与BRIEF特征描述子结合起来，并在它们原来的基础上做了改进与优化。采用FAST(features from accelerated segment test)算法来检测特征点，基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。

得到特征点后我们需要以某种方式描述这些特征点的属性。这些属性的输出我们称之为该特征点的描述子(Feature DescritorS)。在现实生活中，我们从不同的距离、方向、角度、光照条件下观察一个物体时，物体的大小、形状、明暗都会有所不同。但我们的大脑依然可以判断它是同一件物体。理想的特征描述子应该具备这些性质。即，在大小、方向、明暗不同的图像中，同一特征点应具有足够相似的描述子，称之为描述子的可复现性。ORB采用BRIEF算法来计算一个特征点的描述子。

在本实施例中，通过第一图像帧与与第二图像帧的特征点，计算描述子，通过描述子的相似度进行匹配，判断是否为相同的特征点，获取所述第一图像帧与第二图像帧相匹配的ORB特征点作为所述第一图像帧的匹配特征点。

步骤S30，对所述第一图像帧等份划分，得到多个图像块，选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，并通过RANSAC算法获取每个格子模型的内点，及内点的质心。

如何有效的获得外点率低的图像中的静态特征点，而非像深度学习利用纹理、颜色、灰度等来判定运动属性，对于机器人解算相机位姿有着重要意义。事实上，在一个运动物体的视野中，区分静动态目标不是一件简单的事情。原因在于动态物体与静态物体的占比并不明确，同时物体运动和静止是可以相互转换的。我们可以得到下述两个观察：无论是运动物体上的特征点还是静止物体上的特征点都在物体上，因此相邻越近的特征点越有可能同属于一个物体，进而属于一个运动模型。

在本实施例中，将第一图像帧划分为m*n的图像块，其中m为图像在y方向等份划分的数量，n为图像在x方向等份划分的数量，如图3所示。并设m*n将图像块的序列转换为一维描述，选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，统计格子模型的数量，若格子模型的数量大于预设值，则对图像块中的特征点进行归一化，根据归一化后的特征点采用RANSAC算法解算图像块的运动模型，即筛选剔除误匹配的特征点，此处保留符合格子模型的模型内点，执行步骤S40；若所述格子模型的数量小于预设值，则执行步骤S10重新获取两帧RGB图像，重新计算特征点。

步骤S40，对每一个格子模型，基于其内点分别计算其与其他格子模型的耦合度，并基于预设的耦合度阈值选取格子模型，构建对应的格子模型集合。

在SLAM中，运动物体之间的运动状态互斥，与静止物体的运动状态更互斥，因此运动物体的模型分布往往使较为集中的，相反，在现实环境中，获取静态特征点并不占据图像的主要部分，静态特征点的往往遍布于图像中，因此其模型具有跨区域耦合的特性。

在本实例中，基于步骤S30得到的格子模型，构建格子模型的耦合矩阵，通过格子模型的内点计算当前格子模型与其他格子模型的耦合度，若耦合度大于预设的耦合度阈值，将其他格子模型增入到当前格子模型的集合，依次得到每个格子模型的图像块集合。具体步骤如下：

步骤S41，设z为满足模型解算要求的图像块个数，即格子模型，对于z个图像块，建立z*z维度的耦合矩阵；

步骤S42，耦合矩阵中的(i,j)个元素为第j个格子模型内点同时为第i个格子模型的内点的数量与第j个格子模型的模型内点总数的比值，该值在本专利中又被称为图像块相互之间的耦合度。例如，第一个格子模型与第二个格子模型的耦合度为：基于第二个格子模型的内点，通过RANSAC算法获取其符合第一格子模型的内点的数量，将该数量与第二格子模型的内点的数量的比值作为第一个格子模型与第二个格子模型的耦合度。

步骤S43，设定了耦合度阈值，对于第i个图像块，将第i行中耦合度大于阈值的图像块划分为第i个模型图像块集合。

步骤S50，对于每一个格子模型集合，基于其中每个格子模型内点的质心计算分布方差，并选取最大分布方差值对应的格子模型集合，并基于该集合中各格子模型的内点构建静态特征集合。

在一个运动物体(SLAM的主体)的视野中，解算的乃是相对运动状态，在这样的情况下，运动物体与静态物体的差别仅仅在于其相对运动状态。

想象在宇宙空间中，所有物体的运动都是相对的，但我们仍希望找到一个运动状态稳定且分布较广的运动状态，并选取在该状态中的物体建立坐标系，因为如此以来，物体的运动状态得到了保证该问题可得到极大的简化。在相机视野中的景象不断变换的同时，我们希望找到一个可以运动状态稳定且存在稳定的坐标系。因此分布度是衡量该运动坐标系是否为我们所要解算的坐标系的重要依据。而在地球环境中，普遍来说，这种坐标系即为静态坐标系。

本实施例中，基于步骤S40得到的计算z个格子模型的图像块集合，计算图像块集合的分布度函数，即分布方差F_distribution，其计算步骤如下：

分别计算图像块集合中格子模型质心的x均值，y均值；

基于x均值，y均值，计算图像块集合中格子模型质心的x方向方差X_variance，与y方向方差Y_variance；

基于上述的方差，求解计算分布度函数，如公式(1)所示：

选择分布度函数最高的图像块集合为相机的运动模型，基于图像块集合中的内点构建静态特征点集合。

步骤S60，对所述静态特征集合中的内点进行三角化，通过非线性优化方法获取SLAM的三维坐标，并基于该三维坐标对SLAM进行初始化。

在本实施例中，基于步骤S50得到的静态特征集合，将静态特征点集合中的内点进行了三角化，并利用非线性优化的方法对相机运动模型和静态特征点集合中的内点三维坐标进行了优化迭代，求解出SLAM的三维坐标，基于所述三维坐标对SLAM进行初始化。

本发明第二实施例的一种动态环境下的视觉SLAM初始化系统，如图2所示，包括：获取模块100、特征匹配模块200、筛选模块300、构建集合模块400、计算分布方差模块500、初始化模块600；

获取模块100，配置为从输入视频中获取具有视差的第一图像帧、第二图像帧；

特征匹配模块200，配置为对所述第一图像帧、所述第二图像帧，分别进行ORB特征点的提取，通过特征点匹配方法获取所述第一图像帧的匹配特征点；

筛选模块300，配置为对所述第一图像帧等份划分，得到多个图像块，选取所包含的匹配特征点大于设定阈值的图像块作为格子模型，并通过RANSAC算法获取每个格子模型的内点，及内点的质心；

构建集合模块400，配置为对每一个格子模型，基于其内点分别计算其与其他格子模型的耦合度，并基于预设的耦合度阈值选取格子模型，构建对应的格子模型集合；

计算分布方差模块500，配置为对于每一个格子模型集合，基于其中每个格子模型内点的质心计算分布方差，并选取最大分布方差值对应的格子模型集合，并基于该集合中各格子模型的内点构建静态特征集合；

初始化模块600，配置为对所述静态特征集合中的内点进行三角化，通过非线性优化方法获取SLAM的三维坐标，并基于该三维坐标对SLAM进行初始化。

所述技术领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的系统的具体的工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

需要说明的是，上述实施例提供的动态环境下的视觉SLAM初始化系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。

本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适用于由处理器加载并实现上述的动态环境下的视觉SLAM初始化方法。

本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的动态环境下的视觉SLAM初始化方法。

所述技术领域的技术人员可以清楚的了解到，未描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。