CN110599545B - 一种基于特征的构建稠密地图的系统 - Google Patents

Abstract

本发明涉及一种基于特征的构建稠密地图的系统，涉及图像分析领域。包括跟踪线程、滤波线程、局部地图线程和回环检测线程；所述跟踪线程用于获取图像并提取特征点，通过所述追踪每帧所述图像中的位姿，所述跟踪线程还用于根据所述特征点和所述位姿对所述图像进行关键帧提取得到初始局部地图，并根据所述初始局部地图判断是否需要插入关键帧，所述跟踪线程还用于需要插入关键帧的时候根据预设方案得到插入关键帧的时间点；所述滤波线程用于根据所述时间点插入关键帧，所述滤波线程还用于提取所述初始局部地图中的关键帧并进行滤波，得到所述初始局部地图中的关键帧的梯度图。本方案解决了如何构建更稠密的地图的技术问题，适用于稠密地图的构建。

Description

一种基于特征的构建稠密地图的系统

技术领域

本发明涉及图像分析领域，特别涉及一种基于特征的构建稠密地图的系统。

背景技术

近年来，人工智能的发展如火如荼，在AI算法发展的同时，其落地需要寻找载体，于是SLAM(Simultaneous Localization and Mapping，同时定位与建图)重新引起人们的注意。SLAM是机器人完成自主移动的一项核心技术，该技术使机器人能够在未知环境下利用所携带的传感器实现对自身的定位，同时完成对周围环境的建图。SLAM用到传感器主要有激光传感器，视觉传感器，激光传感器价格昂贵，精度高，但难以获取环境中物体的表面信息；视觉传感器具有体积小、成本低、易于获取等优点，引起了人们的极大兴趣，在SLAM系统中得到了广泛的应用。

按优化方法分类，VSLAM(Visual SLAM)可以分为基于非线性优化、基于滤波优化。根据前端视觉跟踪方法，Visual SLAM(VSLAM)可以分为直接SLAM和基于特征的SLAM，其中基于特征的SLAM仅使用图像中小部分点的信息，一般可以构建稀疏地图；而直接SLAM利用有关图像的大部分或甚至所有像素的信息来构建(半)密集云图。

基于特征的SLAM提取一系列富含图像特征信息的点，以最小的再投影误差进行姿态估计。该方法具有精度高、计算速度快的特点，由于单帧图像包含的特征点较少，且没有足够的信息来构建地图，所以一般采用稀疏地图来重建场景。由于该方法基于特征，不适合重建纹理不丰富或图像模糊的场景，因此在提取特征点较少的动态场景中鲁棒性较差。

直接法最根本思想是最小化光度误差以估计运动姿态[6,8]。因直接法对特征信息要求不高，甚至没有特征点的概念(不采用光流法而直接对全像素进行光度误差计算)，以至于直接法能够在特征缺失的情况下系统仍可能继续运行，缓解特征依赖,但也不能完全消除。当采用直接SLAM构建稠密地图时，通常一帧图像有30多万个像素点(假设图片分辨率为640x480，则为307200个像素)，由于要对参考帧和当前帧的光度误差做优化，仅一次迭代就需要计算30多万项差值。对于单目视觉，还需要考虑深度估计的计算，这使得运算量过大。除此之外，直接法基于灰度不变假设，认为对应点在不同帧里都是同样的灰度值，对环境要求更高，定位精度普遍不如特征法高。

在VSLAM中，按照视觉传感器的使用，可分为单目、双目和RGBD。单目相机以其低廉的价格被广泛应用于生活的各个领域。但是由于缺少场景的深度信息，单目SLAM具有深度估计的不确定性和绝对尺度丢失的缺点。

双目相机一般由水平放置的左眼相机和右眼相机组成，由于已知基线(两相机间的距离)，在恢复深度时可以得到绝对尺度，但配置和标定均较为复杂，其深度量程和精度受双目的基线与分辨率所限，且视差的计算非常消耗计算资源^[3,5](视差是左右图对应像素的横坐标之差)。根据视差可以估计深度(一个像素与相机之间的距离)，

其中b为基线，d为视差，f为相机光心到像平面的距离。

RGBD相机不仅可以捕获图像信息，还可以提供场景深度。它的量程虽不如激光传感器那般广，但在室内的环境下，RGBD已然够用。应用RGBD可以直接获取图像深度,使得系统拥有绝对尺度，减免系统为了恢复像素点深度的计算。

Covisibility Graph：共视图，是一个无向有权图(Graph)，这个概念最早来自2010的文章[Closing Loops Without Places]。在共视图中将每个关键帧作为一个节点，如果两个关键帧有相同的地图点(即它们有共视点)，就把这两个节点连接起来，连接边的权重就是两个关键帧共享的地图点的个数。局部BA优化依赖的就是一个局部的共视图，全局BA优化依赖的就是一个全局的共视图。

由于共视图中边的数量较多，我们创建共视图的子图-本质图，保留全部的节点，但是选择更少的边。ORB-SLAM系统在第一个关键帧开始就维护一个生成树，每个关键帧只与共视点最多的相邻帧产生边。本质图包含生成树、共视地图点数量超过100的边和回环的边。

综上所述，传统的基于特征的SLAM重构的稀疏地图无法为后续工作(如导航、路径规划等)提供足够的有效信息。

发明内容

本发明所要解决的技术问题是如何构建更稠密的地图。

本发明解决上述技术问题的技术方案如下：一种基于特征的构建稠密地图的系统，包括：跟踪线程、滤波线程、局部地图线程和回环检测线程；

所述跟踪线程用于获取图像并提取特征点，通过所述追踪每帧所述图像中的位姿，所述跟踪线程还用于根据所述特征点和所述位姿对所述图像进行关键帧提取得到初始局部地图，并根据所述初始局部地图判断是否需要插入关键帧，所述跟踪线程还用于需要插入关键帧的时候根据预设方案得到插入关键帧的时间点；

所述滤波线程用于根据所述时间点插入关键帧，所述滤波线程还用于提取所述初始局部地图中的关键帧并进行Scharr滤波，得到所述初始局部地图中的关键帧的梯度图，并根据所述梯度图得到梯度信息，所述滤波线程还用于找出梯度信息大于阈值的所述特征点，所述滤波线程还用于根据梯度信息大于所述阈值的所述特征点找到在深度图内对应的深度信息Z，并找到深度值超过预设值的所述特征点作为忽略特征点，所述忽略特征点还包括无效的所述特征点，所述滤波线程还用于对所述图像进行第一次优化，所述第一次优化忽略所述忽略特征点；所述滤波线程还包括统计滤波器和体素滤波器，所述统计滤波器用于利用统计滤波器去除离群点，所述体素滤波器用于对三维空间进行降采样后获得降采样特征点，将所述降采样特征点存入所述初始局部地图得到云地图；

所述云地图线程用于对每帧所述云地图进行局部BA优化，获取共视图已连接的关键帧作为第一关键帧，搜索第一关键帧中未匹配的ORB特征的对应关系，并三角化所述第一关键帧的点云；

所述回环检测线程用于获取并检测每个所述第一关键帧，对形成闭环的所述第一关键帧计算相似变换得到闭环的累计误差，融合形成闭环的所述第一关键帧对应的点云中的重合部分，所述回环检测线程还用于对相似约束的位姿进行图优化。

本发明的有益效果是：通过增加滤波线程，为初始局部地图增加了特征点来表达更多的场景信息，通过具有更高梯度的图像重建稠密地图。

在上述技术方案的基础上，本发明还可以做如下改进。

进一步，所述跟踪线程包括位置识别模块，所述位置识别模块用于全局重定位，获取所述图像的特征点和最初相机位姿，并进行关键帧提取得到初始局部地图。

采用上述进一步方案的有益效果是，这样设置后，在跟踪线程出现跟踪丢失的情况后，能够通过位置识别模块对全局重新进行定位跟踪，完成初始局部地图的提取。

进一步，所述局部地图线程还用于根据点云筛选策略删除冗余关键帧。

进一步，所述阈值为10000。

进一步，所述滤波线程还用于根据下述相机模型

恢复该点在相机坐标系下的位置P_c＝[X,Y,Z]^T，P_c∈3×1，并利用所述P_c进行坐标变换得到变换矩阵，结合所述变换矩阵和所述统计滤波去除所述离群点；

其中[f_x,f_y]是沿xy轴的图像焦距，

是镜头光心在图像中的位置，P_w是指某一点的世界坐标。[u,v]为该点在图像像素平面的坐标，Z为深度信息Z。

进一步，所述坐标变换通过公式

将P_uv变换成P_w，其中

称为变换矩阵。表示从世界坐标系W到相机坐标系C的变换，K为内参矩阵。

本发明附加的方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明实践了解到。

附图说明

图1为本发明基于特征的构建稠密地图的系统的实施例的系统结构示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

实施例基本如附图1所示：

本实施例中基于特征的构建稠密地图的系统，包括：跟踪线程、滤波线程、局部地图线程和回环检测线程；

跟踪线程用于获取图像并提取特征点，通过追踪每帧图像中的位姿，跟踪线程还用于根据特征点和位姿对图像进行关键帧提取得到初始局部地图，并根据初始局部地图判断是否需要插入关键帧，跟踪线程还用于需要插入关键帧的时候根据预设方案得到插入关键帧的时间点，本实施例中的预设方案为关键帧筛选准则，关键帧筛选准则具体为：

(1)距离上一次全局重定位超过20帧；

(2)局部建图线程处于空闲，或者距离上一次关键帧插入超过20帧；

(3)当前帧至少能追踪50个地图点；

(4)当前帧能追踪到的地图点要少于参考关键帧Kf中地图点的90％。

其中条件1确保一个良好的重定位，条件3保证一个良好的追踪，条件4保证了一个最小的视角变化。其中跟踪线程除了计算每帧的位姿，还有一个很重要的目的是确定当前帧是否可作为关键帧。由于在局部建图线程中会剔除冗余的关键帧，所以在关键帧插入时应该尽可能的迅速，从而使得追踪线程对相机的运动(尤其是旋转运动)更具有鲁棒性。图像插入频率过高会导致信息冗余度快速增加，而这些冗余的信息对系统的精度却十分有限，甚至没有提高，反而消耗了更多的计算资源；

滤波线程用于根据时间点插入关键帧，滤波线程还用于提取初始局部地图中的关键帧并进行Scharr滤波，得到初始局部地图中的关键帧的梯度图，并根据梯度图得到梯度信息，滤波线程还用于找出梯度信息大于阈值的特征点，滤波线程还用于根据梯度信息大于阈值的特征点找到在深度图内对应的深度信息Z，本实施例中的系统只适配有RGBD信息的系统，从RGBD信息中获取深度图；并找到深度值超过预设值的特征点作为忽略特征点，忽略特征点还包括无效的特征点，滤波线程还用于对图像进行第一次优化，第一次优化忽略忽略特征点；滤波线程还包括统计滤波器和体素滤波器，统计滤波器用于利用统计滤波器去除离群点，体素滤波器用于对三维空间进行降采样后获得降采样特征点，将降采样特征点存入初始局部地图得到云地图；

云地图线程用于对每帧云地图进行局部BA优化，获取共视图已连接的关键帧作为第一关键帧，搜索第一关键帧中未匹配的ORB特征的对应关系，并三角化第一关键帧的点云；

回环检测线程用于获取并检测每个第一关键帧，对形成闭环的第一关键帧计算相似变换得到闭环的累计误差，融合形成闭环的第一关键帧对应的点云中的重合部分，回环检测线程还用于对相似约束的位姿进行图优化。

本发明的有益效果是：通过增加滤波线程，为初始局部地图增加了特征点来表达更多的场景信息，通过具有更高梯度的图像重建稠密地图。

在上述技术方案的基础上，本发明还可以做如下改进。

可选的，在一些其它实施例中，跟踪线程包括位置识别模块，位置识别模块用于全局重定位，获取图像的特征点和最初相机位姿，并进行关键帧提取得到初始局部地图。

这样设置后，在跟踪线程出现跟踪丢失的情况后，能够通过位置识别模块对全局重新进行定位跟踪，完成初始局部地图的提取。

可选的，在一些其它实施例中，局部地图线程还用于根据点云筛选策略删除冗余关键帧，本实施例中的点云筛选策略包括两个条件：(1)超过25％的帧数在理论上可以观察到该地图点；(2)该地图点创建后能够被至少三个连续关键帧追踪到。本实施例中的删除冗余关键帧具体为：选择局部建图线程中正在处理的共视关键帧作为候选的关键帧，不包括第一帧关键帧与当前关键帧。具体如下：如果一个关键帧检测到的90％的地图点，在其他不少于三个关键帧里面都被检测到，就认定该关键帧冗余(该关键帧的存在提供的地图点观测信息有限)，并剔除。

可选的，在一些其它实施例中，阈值为10000。

可选的，在一些其它实施例中，滤波线程还用于根据下述相机模型

恢复该点在相机坐标系下的位置P_c＝[X,Y,Z]^T，P_c∈3×1，并利用P_c进行坐标变换得到变换矩阵，结合变换矩阵和统计滤波去除离群点；

其中[f_x,f_y]是沿xy轴的图像焦距，

是镜头光心在图像中的位置，P_w是指某一点的世界坐标。[u,v]为该点在图像像素平面的坐标，Z为深度信息Z。

可选的，在一些其它实施例中，坐标变换通过公式

将P_uv变换成P_w，其中

称为变换矩阵。表示从世界坐标系W到相机坐标系C的变换，K为内参矩阵。

需要说明的是，上述各实施例是与上述各方法实施例对应的产品实施例，对于本实施例中各结构装置及可选实施方式的说明可以参考上述各方法实施例中的对应说明，在此不再赘述。

读者应理解，在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (6)

1.一种基于特征的构建稠密地图的系统，其特征在于，包括：

跟踪线程、滤波线程、局部地图线程和回环检测线程；

所述跟踪线程用于获取图像并提取特征点，通过追踪每帧所述图像中的位姿，所述跟踪线程还用于根据所述特征点和所述位姿对所述图像进行关键帧提取得到初始局部地图，并根据所述初始局部地图判断是否需要插入关键帧，所述跟踪线程还用于需要插入关键帧的时候根据预设方案得到插入关键帧的时间点；

所述滤波线程用于根据所述时间点插入关键帧，所述滤波线程还用于提取所述初始局部地图中的关键帧并进行Scharr滤波，得到所述初始局部地图中的关键帧的梯度图，并根据所述梯度图得到梯度信息，所述滤波线程还用于找出梯度信息大于阈值的所述特征点，所述滤波线程还用于根据梯度信息大于所述阈值的所述特征点找到在深度图内对应的深度信息Z，并找到深度值超过预设值的所述特征点作为忽略特征点，所述忽略特征点还包括无效的所述特征点，所述滤波线程还用于对所述图像进行第一次优化，所述第一次优化忽略所述忽略特征点；所述滤波线程还包括统计滤波器和体素滤波器，所述统计滤波器用于利用统计滤波器去除离群点，所述体素滤波器用于对三维空间进行降采样后获得降采样特征点，将所述降采样特征点存入所述初始局部地图得到云地图；

所述云地图线程用于对每帧所述云地图进行局部BA优化，获取共视图已连接的关键帧作为第一关键帧，搜索第一关键帧中未匹配的ORB特征的对应关系，并三角化所述第一关键帧的点云；

所述回环检测线程用于获取并检测每个所述第一关键帧，对形成闭环的所述第一关键帧计算相似变换得到闭环的累计误差，融合形成闭环的所述第一关键帧对应的点云中的重合部分，所述回环检测线程还用于对相似约束的位姿进行图优化。

2.根据权利要求1所述的基于特征的构建稠密地图的系统，其特征在于：所述跟踪线程包括位置识别模块，所述位置识别模块用于全局重定位，获取所述图像的特征点和最初相机位姿，并进行关键帧提取得到初始局部地图。

3.根据权利要求1所述的基于特征的构建稠密地图的系统，其特征在于：所述局部地图线程还用于根据点云筛选策略删除冗余关键帧。

4.根据权利要求1所述的基于特征的构建稠密地图的系统，其特征在于：所述阈值为10000。

5.根据权利要求1所述的基于特征的构建稠密地图的系统，其特征在于：

所述滤波线程还用于根据下述相机模型

恢复该点在相机坐标系下的位置P_c＝[X,Y,Z]^T，P_c∈3×1，并利用所述P_c进行坐标变换得到变换矩阵，结合所述变换矩阵和所述统计滤波去除所述离群点；

其中[f_x,f_y]是沿xy轴的图像焦距，

是镜头光心在图像中的位置，P_w是指某一点的世界坐标, [u,v]为该点在图像像素平面的坐标，Z为深度信息Z。

6.根据权利要求5所述的基于特征的构建稠密地图的系统，其特征在于：

所述坐标变换通过公式

将P_uv变换成P_w，其中

称为变换矩阵, 表示从世界坐标系W到相机坐标系C的变换，K为内参矩阵。

Images