动静态场景的定位和建图方法、装置、设备和存储介质与流程

本发明实施例涉及计算机视觉技术，尤其涉及一种动静态场景的定位和建图方法、装置、设备和存储介质。

背景技术：

近年来，由于相机的低成本和轻量级特性，视觉里程计(visualodometry，vo)和视觉同时定位和建图(visualsimultaneouslocalizationandmapping，vslam)在机器人定位和导航中有着重要的地位。

但在复杂的环境中，vo和vslam要完成精准和鲁棒的定位导航任务，面临着极大的挑战。动态场景作为复杂环境的一种，其动态物体极大地影响着vo和vslam的轨迹精度和系统鲁棒性，如果不滤除动态物体而保持着动态物体的影响，那么系统估计的轨迹可能离正确的轨迹越偏越远。而且，在现实生活中，很难出现完全静止的场景，总会出现各种各样的动态物体，如人、车、动物等等，vo和vslam要应用在现实场景中，那么动态场景是无法避免的一个问题。

目前解决动态场景的主要思路有两种，一种是基于先验语义信息，用神经网络来分割出图像中的运动目标，如人、车等，系统事先知道这些语义信息，然后利用判别算法区分动态目标和静态背景；另一种是基于运动一致性判别，在提取到的特征点或者划分出来的块上面做划分，判断每一个地图点或者块是静态点还是动态点。前者分割比较精准，一旦判断为动态目标，则可以滤除整个该动态目标对系统的影响，但是对训练数据有严重依赖性，泛化能力差，也就是说，对于神经网络没有学习到的动态目标，该方法是无法分割出来的，另一方面，该方法需要在图形处理器(graphicsprocessingunit，gpu)上面运行，在一些低性能的手持设备上很难应用。后者对建立的地图点做处理，它的泛化能力较强，适应于所有类型的动态目标场景中，并且可以在cpu上面运行，但每次优化只能滤除一部分动态点，一些动态点还是会被建到局部地图中，在后面的位姿优化中，继续用这些暂时未滤除的动态点来做优化，会影响位姿精度，另外，目前采用的基于三维空间的分块方法，分割效果较差，在动态物体靠近静态背景时，存在误分割现象，也会影响位姿精度。

综上所述，目前针对复杂场景中的定位和建图方法均具有一定的使用限制，无法被广泛应用。

技术实现要素：

本发明实施例提供一种动静态场景的定位和建图方法、装置、设备和存储介质，降低了动态点对建图的影响，且无需消耗大量的计算资源。

第一方面，本发明实施例提供一种动静态场景的定位和建图方法，包括：

依次获取相机拍摄的图像帧；

筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿；

对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点；

根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图。

在第一方面一种可能的实现方式中，根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图之后，还包括：

对关键帧进行回环检测，优化系统维护的局部地图和关键帧对应的相机位姿。

在第一方面一种可能的实现方式中，筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿，包括：

对当前图像帧进行追踪；

若当前图像帧追踪成功，且当前图像帧的上一帧追踪成功，则基于当前图像帧的上一帧对当前图像帧进行相机位姿估计；

若当前图像帧追踪成功，且当前图像帧的上一帧追踪失败，则基于当前图像帧的上一帧的全局重定位结果对当前图像帧进行相机位姿估计；

若当前图像帧追踪失败，则基于全局重定位对当前图像帧进行相机位姿估计；

追踪局部地图对估计出的相机位姿进行优化得到当前图像帧的相机位姿并优化局部地图；

当当前图像帧满足预设关键帧条件时将当前图像帧作为关键帧。

在第一方面一种可能的实现方式中，对当前图像帧进行追踪，包括：

提取当前图像帧的orb特征；

将当前图像帧的orb特征与当前图像帧上一帧的orb特征进行特征匹配，若当前图像帧的orb特征与当前图像帧上一帧的orb特征的匹配数量大于预设阈值，则当前图像帧追踪成功。

在第一方面一种可能的实现方式中，提取当前图像帧的orb特征之后，还包括：

对当前图像帧与当前图像帧上一帧匹配的特征点附近预设大小的图像块计算ncc；

确定ncc值低于预设阈值的图像块对应的当前图像帧中的特征点为误匹配特征点，

在系统维护的局部地图中滤除误匹配特征点对应的地图点。

在第一方面一种可能的实现方式中，当当前图像帧满足预设关键帧条件时将当前图像帧作为关键帧，包括：

当当前图像帧满足以下条件至少之一时，将当前图像帧作为关键帧：

当前图像帧距离上一次全局重定位超过预设数量个图像帧；

当前图像帧距离上一个关键帧超过预设数量个图像帧；

当前图像帧跟踪局部地图中对应的地图云点少于预设数量；

对根据关键帧进行局部建图的处理未同步进行。

在第一方面一种可能的实现方式中，筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿之前，还包括：

若当前图像帧为相机获取的第一个图像帧，则将第一个图像帧作为关键帧，并根据第一个图像帧建立第一图像帧的相机位姿和系统维护的局部地图；

筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿，包括：

筛选出图像帧中的第二个关键帧，并根据第二个关键帧和第一个图像帧进行运动一致性检测，剔除第二个关键帧中的动态特征点，并根据剔除动态特征点的第二个关键帧重新建立系统维护的局部地图和第二个关键帧的相机位姿；

删除第一个图像帧对应的相机位姿和系统维护的局部地图。

在第一方面一种可能的实现方式中，对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点，包括：

对关键帧进行聚类，将关键帧划分为多个区块；

根据聚类处理后的关键帧和关键帧的上一个关键帧进行运动一致性检测，确定关键帧中的动态区块；

在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点。

在第一方面一种可能的实现方式中，对关键帧进行聚类，将关键帧划分为多个区块，包括：

对关键帧进行4维k-means聚类，将关键帧划分为多个区块。

在第一方面一种可能的实现方式中，根据聚类处理后的关键帧和关键帧的上一个关键帧进行运动一致性检测，确定关键帧中的动态区块，包括：

根据上一关键帧，计算聚类处理后的关键帧中每个区块对应的内点率；

将聚类处理后的关键帧中内点率低于预设内点率阈值且面积低于预设面积阈值的区块作为动态区块。

在第一方面一种可能的实现方式中，根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图，包括：

在关键帧和共视图中邻近关键帧中进行特征点的搜索匹配，若搜索到匹配的特征点，该特征点对应的3d点不在系统维护的局部地图中，而且该特征点不是关键帧中动态区块对应的动态特征点，则将该点对应的3d点创建到系统维护的局部地图中，即将关键帧中静态特征点对应的3d点加入系统维护的局部地图中。

第二方面，本发明实施例提供一种动静态场景的定位和建图装置，包括：

图像帧获取模块，用于依次获取相机拍摄的图像帧；

关键帧筛选模块，用于筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿；

聚类模块，用于对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点；

建图模块，用于根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图。

第三方面，本发明实施例提供一种动静态场景的定位和建图设备，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，

当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现如第一方面任一种可能的实现方式的动静态场景的定位和建图方法。

第四方面，本发明实施例提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面任一种可能的实现方式的动静态场景的定位和建图方法。

本发明实施例提供的动静态场景的定位和建图方法、装置、设备和存储介质，通过依次获取相机拍摄的图像帧，筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿，对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点，根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图，实现了相机的定位和同时建图，由于采用聚类的方法将关键帧划分为多个区块后滤除了局部地图中的动态区块中所有特征点对应的地图点，降低了动态点对定位和建图的影响，且无需消耗大量的计算资源。

附图说明

图1为本发明实施例提供的动静态场景的定位和建图方法实施例一的流程图；

图2为本发明实施例提供的动静态场景的定位和建图方法实施例二的流程图；

图3为本发明实施例提供的动静态场景的定位和建图方法对应的系统框图；

图4为本发明实施例提供的动静态场景的定位和建图装置实施例一的结构示意图；

图5为本发明实施例提供的一种动静态场景的定位和建图设备的结构示意图。

具体实施方式

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

图1为本发明实施例提供的动静态场景的定位和建图方法实施例一的流程图，如图1所示，本实施例提供的方法包括：

步骤s101，依次获取相机拍摄的图像帧。

视觉同时定位和建图，即在通过视频拍摄同时完成环境地图的建立和相机的定位(即相机在环境地图中的位置和姿态的确定)，其中相机可以为能够拍摄视频的相机或能够连续拍摄图像的相机。当相机处于运动过程中，连续拍摄的图像帧表征了相机拍摄到的场景与相机之间的位置关系，因此可以通过对相机连续拍摄的图像帧进行处理，进行环境地图的构建，并确定的相机的位姿(即相机在环境地图中的相对位置关系)。

本实施例中的相机可以是任一种能够拍摄图像并获得深度的相机或相机组合，例如可以为rgb-d相机也可以为双目相机。将相机在每个固定的周期拍摄到图像作为一个图像帧，然后需要对每一图像帧进行处理，也就是需要依次获取相机拍摄的图像帧。

步骤s102，筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿。

由于相机在运动过程中持续进行图像帧的拍摄，因此相机与图像帧中各物体的相对位置关系与相机的运动轨迹是相关的，那么通过对相机连续拍摄的图像帧进行分析，就可以确定相机的位姿，同时还可以根据连续拍摄的图像帧构建地图，也即进行视觉同时定位和建图。而由于相机是持续进行图像帧的拍摄，而拍摄的各图像帧之间的间隔一般较短，例如每个图像帧之间间隔0.04秒甚至更短。若在进行视觉同时定位和建图时，使用相机拍摄的所有图像帧，那么将产生极大的计算量，需要进行视觉同时定位和建图的设备具有强大的计算能力并消耗大量的计算资源，而这显然无法大规模的应用。

而由于相机在运动过程中，一般情况下，在一段时间内拍摄到的图像帧中的物体的相似度较高，间隔一段时间的图像帧之间一般具有较大的差别，因此采用相机拍摄的所有图像帧进行视觉同时定位和建图会产生很多冗余的计算。那么就可以对相机依次拍摄的各图像帧进行筛选，仅使用部分图像帧进行视觉同时定位和建图，将进行视觉同时定位和建图计算的图像帧称为关键帧，关键帧表征了相机拍摄的环境的关键特征。每个筛选出的关键帧都表征了相机拍摄的物体的部分关键特征，可以根据一些特定的规则对相机拍摄的图像帧进行筛选，依次确定相机拍摄的图像帧中的各关键帧，然后，依次确定各关键帧对应的相机位姿。

从相机拍摄的图像帧中筛选出关键帧，需要判断当前图像帧是否满足预设的关键帧条件。另外，还可以对当前图像帧进行追踪，根据追踪结果确定当前图像帧对应的相机位姿。对当前图像帧进行追踪就是判断当前图像帧与当前图像帧的上一帧对应的特征点的匹配度是否高于预设阈值。在本实施例中，可以采用orb(orientedfastandrotatedbrief)特征进行匹配，那么对当前图像帧进行追踪的方法具体包括：提取当前图像帧的orb特征，将当前图像帧的orb特征与当前图像帧上一帧的orb特征进行特征匹配，若当前图像帧的orb特征与当前图像帧上一帧的orb特征的匹配数量大于预设阈值，则当前图像帧追踪成功。提取orb特征的具体方法可以为：在8层金字塔提取fast(featuresfromacceleratedsegmenttest)角点，尺度因子为1.2。根据图像分辨率来确定提取角点的数目。为了保证单映射分布，将每层分成网格，每格至少5个角点。根据保留的fast角点计算方向和orb特征描述子，orb特征描述子用于所有的特征匹配。当然，本领域技术人员应能理解上述orb特征仅为举例，其他现有的或今后可能出现的特征匹配方法如可适用于本申请，也应包含在本申请保护范围以内，并在此以引用方式包含于此。

若当前图像帧追踪成功，则继续判断当前图像帧的上一帧是否追踪成功，若当前图像帧的上一帧追踪成功，则基于当前图像帧的上一帧对当前图像帧进行相机位姿估计。例如，如果当前图像帧的上一帧跟踪成功，我们就用运动速率恒定模型来预测当前图像帧对应的相机初始位姿(即认为摄像头处于匀速运动，即，在当前图像帧到来时，由于已经估计出当前图像帧的上一帧的位姿，所以上上帧到上一帧的位姿变化量是已知的，这里认为上一帧到当前图像帧的位姿变化量等于上上帧到上一帧的位姿变化量，上一帧的位姿加上这个变化量作为当前图像帧的初始位姿)，然后搜索地图中的云点与当前图像帧的匹配点，最后利用搜索到的匹配点对当前相机的位姿进一步优化。但是，如果没有找到足够的匹配(比如，运动模型失效)，就加大搜索范围，搜索地图云点附近的点在当前图像帧图像中是否有匹配点，然后通过寻找到的对应匹配点对来优化当前图像帧对应的相机位姿。

若当前图像帧追踪成功，且当前图像帧的上一帧追踪失败，则基于当前图像帧的上一帧的全局重定位结果对当前图像帧进行相机位姿估计。例如，将当前图像帧的上一帧的全局重定位结果作为当前图像帧的相机初始位姿，然后搜索地图中的对应云点与当前图像帧的匹配点，最后利用搜索到的匹配点对当前相机的位姿进一步优化。

若当前图像帧追踪失败，则基于全局重定位对当前图像帧进行相机位姿估计。例如，如果跟踪丢失，把当前图像帧转换成图像词袋，检索图像数据库，为全局重定位查找关键帧。计算orb特征和每个关键帧的地图云点的对应关系。接着，对每个关键帧执行随机抽样一致算法(randomsampleconsensus，ransac)迭代计算，用n点透视投影(perspectivenpoint，pnp)算法找出相机位姿。如果通过足够的有效数据找到相机位姿，优化它的位姿，搜索候选关键帧的地图云点的更多的匹配。最后，相机位置被进一步优化，如果有足够的有效数据，跟踪程序将持续执行。

追踪局部地图对估计出的相机位姿进行优化得到当前图像帧的相机位姿并优化局部地图。一旦获得了相机位姿和一组初始特征匹配的估计，可以将地图和图像帧对应起来，搜索更多地图云点的对应关系。为了减少大地图的复杂性，我们只映射局部地图。相机位姿最后通过当前图像帧中获得所有的地图云点进行优化。

当当前图像帧满足预设关键帧条件时将当前图像帧作为关键帧。

该步骤是决定当前图像帧是否可以作为关键帧。局部地图构建的过程中有一个机制去筛选冗余的关键帧，尽可能快地插入关键帧，这可以使跟踪线程对相机的运动更具鲁棒性，尤其是旋转。根据以下要求至少之一插入新的关键帧：

当前图像帧距离上一次全局重定位超过预设数量个图像帧，例如，距离上一次全局重定位后需要超过20帧图像；

当前图像帧距离上一个关键帧超过预设数量个图像帧，例如上一个关键帧插入时，已经有超过20个帧；

当前图像帧跟踪局部地图中对应的地图云点少于预设数量，例如，当前图像帧跟踪少于50个地图云点；

对根据关键帧进行局部建图的处理未同步进行(即步骤s104未同步进行)，例如，局部建图线程处于空闲状态。

如果当前图像帧满足上述条件至少之一，则认为该帧为关键帧。

进一步地，提取当前图像帧的orb特征之后，还包括：

对当前图像帧与当前图像帧上一帧匹配的特征点附近预设大小的图像块计算归一化互相关(normalizationcrosscorrelation，ncc)；确定ncc值低于预设阈值的图像块对应的当前图像帧中的特征点为误匹配特征点，在系统维护的局部地图中滤除误匹配特征点对应的地图点。其中，误匹配可能是由动态点引起，也可能是其他原因引起。

例如，对于匹配的特征点，可以对特征点附近大小为9*9的图像块计算ncc，公式如下：

上式中，i表示像素灰度值，a(i,j)表示图a中第i行第j列的像素点，b(i,j)表示图b中第i行第j列的像素点。

s(a,b)ncc越接近1，则a和b越相似，认为当s(a,b)ncc低于一定阈值时，该匹配为误匹配，当前图像帧中该特征点为误匹配特征点。然后，在系统维护的局部地图中滤除误匹配特征点对应的地图点。然后，对当前图像帧进行追踪，根据追踪结果确定当前图像帧对应的相机位姿，然后再进行关键帧的筛选。当然，本领域技术人员应能理解上述预设大小为9*9的图像块仅为举例，其它预设大小的图像块如可适用于本申请，也应包含在本申请保护范围以内，并在此以引用方式包含于此。

另外，由于相机在刚开始进行图像帧的拍摄时，并没有已经存在的系统维护的局部地图和初始的相机位姿等信息，那么可以进行如下处理：

若当前图像帧为相机获取的第一个图像帧，则将第一个图像帧作为关键帧，并根据第一个图像帧建立第一图像帧的相机位姿和系统维护的局部地图；筛选出图像帧中的第二个关键帧，并根据第二个关键帧和第一个图像帧进行运动一致性检测，剔除第二个关键帧中的动态特征点，并根据剔除动态特征点的第二个关键帧重新建立系统维护的局部地图和第二个关键帧的相机位姿；删除第一个图像帧对应的相机位姿和系统维护的局部地图。

本申请方法在初始化模块，如果仅根据相机获取的第一个图像帧建立第一个图像帧的相机位姿和系统维护的局部地图，例如，将第一个图像帧置为关键帧，第一个图像帧时刻的相机坐标系作为世界坐标系，第一个图像帧的位姿为单位旋转矩阵和零位移向量，将第一个图像帧上的2d特征点，通过深度信息建到3d局部地图中，获得系统维护的局部地图。但是由于相机拍摄的第一个图像帧(仅一个图像帧)无法进行运动一致性检测，无法剔除局部地图中第一个图像帧中动态点对应的地图点，所以第一个图像帧建出来的地图点中有大量动态目标上的点。通过这些地图点优化出来的第二个关键帧的位姿，必将是不准确的。为了剔除动态点，提高第二个关键帧位姿的精度，对初始化模块进行如下处理：同样，将第一个图像帧置为关键帧，利用深度信息创建地图点。在第二个关键帧挑选出来后，由于有两个关键帧对比，可以进行运动一致性检测，判别出第二个关键帧中的动态目标区域以及位于动态目标区域中的动态点。这时，清除掉由第一个关键帧创建出来的地图点，清空局部地图，将第二个关键帧重置为初始化的第一帧，作为世界坐标系，第二个关键帧的位姿为单位旋转矩阵和零位移向量，将第二个关键帧判断出来的静态点通过深度信息建出3d点，插入到局部地图中，由此获得了初始化模块系统维护的局部地图和第二个关键帧对应的相机位姿。也就是说，我们将第二个关键帧重置为系统初始化成功的第一帧。其中，通过深度信息获得图像帧中2d点对应的3d点，可以是利用深度传感器或其它测距仪，还可以利用三角测量法获得图像帧中2d点对应的3d点，当然，本领域技术人员应能理解上述三角测量法或深度传感器等仅为举例，其它的根据2d点获得对应3d点的方法如可适用于本申请，也应包含在本申请保护范围以内，并在此以引用方式包含于此。

步骤s103，对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点。

在视觉同时定位和建图中，动态物体对建图的影响很大，因此在根据相机拍摄的图像帧进行建图过程中，需要尽量滤除掉图像帧中的动态物体。而在传统的视觉同时定位和建图方法中，对动态物体的滤除效果较差或者需要消耗大量的计算资源。而在本实施例中，并不是对关键帧中的各个特征点是否为动态特征点进行一一判断，而是对关键帧进行聚类后划分为多个区块，再对各区块进行判断，确定关键帧中的动态区块，一个区块中包括若干特征点，若一个区块被判断是动态区块，则该区块中的特征点都将被认为是动态特征点。在确定了关键帧中的动态区块后，即可在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点，其中系统维护的局部地图是根据筛选出的关键帧建立的，其中保留了各关键帧中具有共视关系的3d点，也就是局部地图表征了相机拍摄到的图像中的显著的特征点。在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点，能够维持局部地图中的3d点都是静态点，避免动态物体对定位和建图的影响。

由于对关键帧进行聚类后划分为多个区块，并确定关键帧中的动态区块，而不是一一判断关键帧中的特征点是否为动态特征点，这样大大减少了判断是否为动态特征点的计算量，并且能够有效地滤除更多的动态特征点，避免动态特征点对定位和建图的影响。

其中，对关键帧进行聚类，首先需要通过深度信息获得关键帧中2d点对应的3d点，然后对关键帧中的3d点进行聚类，将关键帧划分为多个区块。例如，关键帧中一副图像中所有的有深度信息的2d点，通过深度信息获得的3d点。假设一副关键帧图像有640*480个点，可选地，还可以进行采样(例如每8个点采一个样)以降低3d点的数量。

其中，可以对关键帧进行4维k-means聚类，将关键帧划分为多个区块。k-means算法是一种聚类算法，所谓聚类，即根据相似性原则，将具有较高相似度的数据对象划分至同一类簇，将具有较高相异度的数据对象划分至不同类簇。k-means算法中的k代表类簇个数，means代表类簇内数据对象的均值(这种均值是一种对类簇中心的描述)，因此，k-means算法又称为k-均值算法。k-means算法是一种基于划分的聚类算法，以距离作为数据对象间相似性度量的标准，即数据对象间的距离越小，则它们的相似性越高，则它们越有可能在同一个类簇。数据对象间距离的计算有很多种，通常采用的是2维或者3维的欧式距离来计算数据对象间的距离。在本实施例中，创新性地采用4维的欧式距离作为图像像素间的距离，所谓4维，是点的3d信息加上一个灰度值信息，这样才可以达到比较接近语义信息的划分。聚类算法原理如下：

上式中，dist表示4维欧式距离，x、y、z表示3d点，gray表示灰度值。μk表示第k个类簇中心，ck为属于第k个类簇的所有点的集合，|ck|表示第k个类簇中点的个数。j表示所有类的总聚类平方和，k表示类簇个数，n表示该类簇中的图像点数目。

4维k-means聚类算法的输入为关键帧中的3d点和灰度值，以及聚类的类簇个数和聚类终止阈值，输出为聚类后的多个类簇，每个簇类中包括多个图像帧中的3d点。其中聚类采用上述公式(1)、公式(2)和公式(3)三个公式，分别为针对每个3d点和对应的灰度值，根据公式(1)计算4维欧氏距离，根据计算出的欧式距离将各3d点划分至距离其最近的类簇中心所在类簇中，然后根据公式(2)更新类簇中心，再根据公式(3)计算所有类的总聚类平方和，当总聚类平方和低于聚类终止阈值时完成聚类。

例如：4维k-means聚类算法可以采用如下伪代码进行处理：

输入：t个图像3d点和灰度值，类簇个数k，终止阈值λ。

输出：k个簇类。

初始化类簇中心

fort＝1,2,…,t

foreveryxt(对于所有数据对象)

根据公式(1)计算4维欧式距离；

将xt划分至距离其最近的类簇中心所在类簇中；

endfor

根据公式(2)更新类簇中心；

根据公式(3)计算所有类的总聚类平方和j；

ifj<λ

输出聚类结果；

break；

endfor

接着根据聚类处理后的关键帧和关键帧的上一个关键帧进行运动一致性检测，确定关键帧中的动态区块，也即对聚类后的关键帧进行运动一致性检测。

将聚类后的多个3d点确定为一个区块，对关键帧进行运动一致性检测的输入为聚类后的各区块和每一区块中的3d点集合，输出为每个区块是否为动态区块的标签。对关键帧进行运动一致性检测首先计算每个区块的内点率，然后判断内点率低于一定阈值，且内点率低于一定阈值的区块的面积和低于一定阈值，才认为该块为动态块。

例如：对关键帧进行运动一致性检测可以采用如下伪码所示：

输入：区块{m1,m2,…mn}和每一区块中的3d点集合{p1,p2…pn}

输出：每个区块的标签(是否为动态块)

步骤1:计算内点率

forpi＝p1,…pn

对pi中每一个点投影到上一关键帧，计算重投影误差d

计算pi中满足d<σd条件的点数ni

计算内点率ri＝ni/ci.size()(ci.size()为该区块内有效点数)

步骤2:判断动态区域

endfor

formi＝m1,…mn

计算mi中所有像素个数si

标记mi为静态

endfor

根据内点率ri由小到大排列{m1,m2,…mn}

ssum＝0

while(ssum＜σs&&ri＜σr)

标记mi为动态

ssum＝ssum+si

从上述伪码中可以看到，不只是根据内点率来判断某块是否为动态块，而是加入了动态块的面积联合判断。只有当内点率低于一定阈值，且已标记为动态块的面积和低于一定阈值，才认为该块为动态块。

也就是根据聚类处理后的关键帧和关键帧的上一个关键帧进行运动一致性检测，确定关键帧中的动态区块，包括：根据上一关键帧，计算聚类处理后的关键帧中每个区块对应的内点率；将聚类处理后的关键帧中内点率低于预设内点率阈值且面积低于预设面积阈值的区块作为动态区块。

接着在系统维护的局部地图中滤除动态块内的所有特征点对应的地图点。在运动一致性检测之后，我们可以获得位于动态块内的所有特征点，利用深度信息将其转换为3d点。然后搜索局部地图中有无相同3d点，若有，则删除掉。

步骤s104，根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图。

在对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点之后，该关键帧就可以参与进行局部建图。局部建图主要工作是判断该关键帧中是否有需要加入局部地图的新的3d点，并且还可以根据关键帧与构建局部地图所使用的其他关键帧之间的关系，优化关键帧对应的相机位姿和系统维护的局部地图，从而实现了视觉同时定位和建图，即得到了相机位姿和地图。

根据关键帧进行局部建图的方法可以包括：在关键帧和共视图中邻近关键帧中进行特征点的搜索匹配，若搜索到匹配的特征点，该特征点对应的3d点不在系统维护的局部地图中，而且该特征点不是关键帧中动态区块对应的动态特征点，则将该点对应的3d点创建到系统维护的局部地图中，即将关键帧中静态特征点对应的3d点加入系统维护的局部地图中。

更详细地，根据关键帧进行局部建图的方法可以包括如下步骤：

一、在插入关键帧之后，更新共视图，添加节点，更新关键帧间具有相同地图云点产生的边缘。还要更新生成树上当前关键帧和其他关键帧的连接。然后，计算表示关键帧的词袋，用于数据关联来三角化或根据深度传感器获得新的云点。

二、进行地图云点筛选，保留在地图里的地图云点，在最初创建的3个关键帧上，需要经过严格的检测，保证它们可以被跟踪，不会由于错误的数据被错误地被插入到局部地图。一个云点必须满足的条件如下：

1、在对关键帧进行筛选的步骤中(即步骤s102)，在预测可见的所有帧中有25％以上的帧能观测到该点。

2、如果超过一个关键帧完成地图云点创建过程，它必须至少是能够被其他3个关键帧可被观测到。

一旦一个地图云点通过测试，它只能在被少于3个关键帧观测到的情况下移除。地图云点被移除的过程可能发生在关键帧的挑选和局部ba优化滤除无效观测点的时候。这个策略使得局部地图包含很少的无效数据。

三、滤除动态点和创建新的地图云点

在当前关键帧和共视图中邻近关键帧中搜索匹配，对于局部地图中没有的云点，若搜索到匹配，而且被动态一致性检测为静态点，则通过深度信息将该点创建到局部地图中。

四、局部光束法平差(bundleadjustment，ba)优化

局部ba优化当前关键帧和与它在共视图中相连接的关键帧的位姿，以及由这些关键帧观测到的所有地图点。所有其他观测到这些地图点但是没有连接到当前关键帧的关键帧，会被加入到优化中，但是他们的状态量(主要是位姿)保持不变。

五、局部关键帧筛选

为了使重构保持简洁，局部地图构建尽量检测冗余的关键帧，删除它们。这会大有帮助，随着关键帧数量的增加，ba优化的复杂度增加，但关键帧的数量也不会无限制地增加，因为它要运行在同一环境下，除非场景内容发生变化。如果某个关键帧观测到的地图点中超过90％点可以被其他3个或以上关键帧在同一尺度或者更好尺度上观测到，会在关键帧数据库中删除该关键帧。

本实施例提供的动静态场景的定位和建图方法，通过依次获取相机拍摄的图像帧，筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿，对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点，根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图，实现了相机的定位和同时建图，由于采用聚类的方法将关键帧划分为多个区块后滤除了局部地图中的动态区块中所有特征点对应的地图点，降低了动态点对定位和建图的影响，且无需消耗大量的计算资源。

图2为本发明实施例提供的动静态场景的定位和建图方法实施例二的流程图，如图2所示，本实施例提供的方法包括：

步骤s201，依次获取相机拍摄的图像帧。

步骤s202，筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿。

步骤s203，对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点。

步骤s204，根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图。

步骤s205，对关键帧进行回环检测，优化系统维护的局部地图和关键帧对应的相机位姿。

在根据图1所示实施例完成了定位和建图后，还可以对关键帧进行回环检测，进一步地优化系统维护的局部地图和关键帧对应的相机位姿。回环检测是同时定位和建图中的一个优化处理过程，在相机到达之前到达的过的位置时，相机拍摄到的物体应该与之前拍摄的物体相同，但是由于位姿估计误差或系统误差等原因，会导致相机在相同位置拍摄的图像帧具有偏差。那么就可以对关键帧进行回环检测，判断系统存储的关键帧数据库中是否存在与当前关键帧关联的回环关键帧，然后再根据回环关键帧所对应的位姿对系统维护的局部地图和关键帧对应的相机位姿进行优化。

具体地，对关键帧进行回环检测可以包括以下步骤：

首先进行候选回环检测，即确定若干候选回环帧，候选回环帧是从之前保存的关键帧库里面计算出来和当前关键帧相似度比较高的一些关键帧。先计算当前关键帧的词袋向量和它在视图内容相关的所有邻近图像中计算相似度，保留最低分值smin。然后，检索图像识别数据库，丢掉那些分值低于smin的关键帧。这和dbow2(图像序列中快速位置识别的二进制词袋)中均值化分值的操作类似，可以获得好的鲁棒性，它计算的是前一帧(上一帧)图像，而本方法是使用的内容相关信息。另外，所有连接到当前关键帧的关键帧都会从结果中删除，即连接到当前关键帧的，也就是当前关键帧附近的帧相似度肯定很高，但这不是回环。比如，上一个关键帧和当前关键帧相似度肯定高，但是不是要检测的回环。为了获得候选回环，可以连续检测3个一致的候选回环。如果对当前关键帧来说环境样子都差不多，就可能有多个候选回环帧。

接着计算相似变换，闭合回环，需要计算从当前关键帧到回环关键帧的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。先计算orb特征关联的当前关键帧的地图云点和回环候选关键帧的对应关系。此时，对每个候选回环，有了一个3d到3d的对应关系。对每个候选回环执行ransac迭代，通过horn(用单位四元数求绝对旋转的闭式解)方法找到相似变换。如果用足够的有效数据找到相似变换，就可以优化它，并搜索更多的对应关系。如果sil(相似变换)有足够的有效数据，再优化它，直到该回环关键帧被接受。

然后进行回环融合，回环矫正的第一步是融合重复的地图云点，在共视图中插入新的边。先通过相似变换矫正当前关键帧位姿，这种矫正方法应用于所有与当前关键帧相邻的关键帧，执行相似变换，这样回环两端就可以对齐。回环关键帧和邻近关键帧所有的地图云点被投影到当前关键帧，在投影点较小的区域内搜索它的近邻和匹配。所有匹配的地图云点和计算相似变换过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们的边。

最后进行本质图优化，为了有效地闭合回环，通过本质图优化位姿图，优化程序通过相似变换校正尺度偏移。优化过后，每一个地图云点都根据关键帧的校正进行变换。

根据图1和图2所示实施例可知，本发明实施例提供的动静态场景的定位和建图方法可以包括如下几个处理步骤，分别为追踪图像帧以确定关键帧、剔除关键帧中动态区域在局部地图中的地图点、根据关键帧进行局部建图、回环检测。那么可以根据上述四个步骤对本发明实施例提供的动静态场景的定位和建图方法构建一个与本发明实施例提供的动静态场景的定位和建图方法对应的系统框图，如在图3中所示，图3为本发明实施例提供的动静态场景的定位和建图方法对应的系统框图。

在图3中将本发明实施例提供的动静态场景的定位和建图方法分为四个线程，分别为追踪线程、动态目标处理线程、局部建图线程和回环检测线程。分别用于执行上述四个步骤。

其中相机拍摄的每个图像帧均先进入追踪线程，依次执行提取orb特征、初步滤除动态点、从上一帧或重定位做初始位姿估计、追踪局部地图、筛选关键帧的处理，最终将筛选出的关键帧输入动态目标处理线程。其中，初步滤除动态点的步骤也可以选择省略。

在动态目标处理线程中，对输入的关键帧进行4维k-means聚类、动态区域判别、擦除动态地图点的处理，然后将该关键帧输入局部建图线程。

在局部建图线程中，对于输入的关键帧，首先进行最新地图点的筛选，然后滤除新的动态点，接着创建新的有效点，然后进行局部ba优化和局部关键帧筛选。

在回环检测线程中，对于进行完局部建图的关键帧，首先检测回环候选帧，然后计算相似变换，接着进行回环融合和本质图优化。

上述四个线程并行进行工作。

另外，由于第一帧的特殊性，在追踪线程中还可以存在初始化过程，对于相机输入的第一个图像帧进行初始化处理。对于动态目标处理线程中的动态区域判别，可以采用本发明实施例提供的运动一致性检测方法进行检测处理。

图4为本发明实施例提供的动静态场景的定位和建图装置实施例一的结构示意图，如图4所示，本实施例提供的动静态场景的定位和建图装置包括：

图像帧获取模块41，用于依次获取相机拍摄的图像帧。

关键帧筛选模块42，用于筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿。

聚类模块43，用于对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点。

建图模块44，用于根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图。

本实施例提供的动静态场景的定位和建图装置用于实现图1所示实施例的动静态场景的定位和建图方法，其实现原理和技术效果类似，此处不再赘述。

图5为本发明实施例提供的一种动静态场景的定位和建图设备的结构示意图，如图5所示，该动静态场景的定位和建图设备包括处理器51、存储器52、摄像头53；动静态场景的定位和建图设备中处理器51的数量可以是一个或多个，图5中以一个处理器51为例；动静态场景的定位和建图设备中的处理器51、存储器52、摄像头53可以通过总线或其他方式连接，图5中以通过总线连接为例。

存储器52作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本申请图1和图2实施例中的动静态场景的定位和建图方法对应的程序指令/模块(例如，动静态场景的定位和建图装置中的图像帧获取模块41、关键帧筛选模块42、聚类模块43、建图模块44)。处理器51通过运行存储在存储器52中的软件程序、指令以及模块，从而执行动静态场景的定位和建图设备的各种功能应用以及数据处理，即实现上述的动静态场景的定位和建图方法。

存储器52可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据动静态场景的定位和建图设备的使用所创建的数据等。此外，存储器52可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。

摄像头53为任一种能够进行图像拍摄的元件或模块。

本申请实施例还提供一种包含计算机可执行指令的存储介质，计算机可执行指令在由计算机处理器执行时用于执行一种动静态场景的定位和建图方法，该方法包括：

依次获取相机拍摄的图像帧；

筛选出图像帧中的关键帧，并确定关键帧对应的相机位姿；

对关键帧进行聚类后划分为多个区块，确定关键帧中的动态区块，在系统维护的局部地图中滤除动态区块内的所有特征点对应的地图点；

根据关键帧进行局部建图，优化关键帧对应的相机位姿和系统维护的局部地图。

当然,本申请实施例所提供的一种包含计算机可执行指令的存储介质,其计算机可执行指令不限于如上的方法操作,还可以执行本申请任意实施例所提供的头戴式显示设备的方法中的相关操作。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。