WO2022002150A1

WO2022002150A1 - 一种视觉点云地图的构建方法、装置

Info

Publication number: WO2022002150A1
Application number: PCT/CN2021/103653
Authority: WO
Inventors: 易雨亭; 李建禹; 龙学雄; 党志强
Original assignee: 杭州海康机器人技术有限公司
Priority date: 2020-06-30
Filing date: 2021-06-30
Publication date: 2022-01-06
Also published as: CN111795704B; CN111795704A

Abstract

一种视觉点云地图的构建方法及装置，方法包括：对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧的特征点；对源图像帧进行帧间跟踪，确定关键帧；将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点；计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。构建地图的过程将建图和定位进行分离，有效地除去了建图与定位的相互影响，对于在复杂多变的环境中，具有更好的适应性和稳定性；装置与方法相对应。

Description

一种视觉点云地图的构建方法、装置

本申请要求于2020年6月30日提交中国专利局、申请号为20201061570.6发明名称为“一种视觉点云地图的构建方法、装置”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。

技术领域

本申请涉及导航定位领域，特别地，涉及一种视觉点云地图的构建方法、装置。

背景技术

构建地图与定位是即时定位与建图(Simultaneous Localization And Mapping，SLAM)研究中的重点技术，而构建地图是实现定位的前提条件，地图的好坏直接影响到定位的精度。视觉点云地图是所构建的一种地图。视觉点云地图通过空间中的三维点集，描述环境中点的视觉、位姿等信息，故而，构建视觉点云地图需要两类数据信息：关键帧和地图点，其中，关键帧描述环境中点的视觉，地图点描述环境中点的位姿。其中，由大量地图点形成的集合构成了点云。

SLAM是指：机器人从未知环境的未知位置出发，在运动过程中，通过重复观测到的地图特征，定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目的。

基于SLAM的地图构建，从输入而言，在机器人运动之前，没有输入，机器人开始运动的时候，有传感器原始数据输入；从输出而言，输出估计位姿和估计地图。可见，相关技术中在建立新地图或者改进已知地图的同时，在该地图上定位机器人。这类似于把一个人放到陌生的城市，让这个人熟悉该城市的过程。基于上述可知，相关技术的SLAM的地图构建将建图问题和定位问题耦合在一起，建图和定位二者相互影响。

发明内容

本申请实施例提供了一种视觉点云地图的构建方法、装置，以避免定位对建图的影响。

本申请实施例提供的一种视觉点云地图的构建方法是这样实现的：

对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点；

对源图像帧进行帧间跟踪，确定关键帧；

将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点；

计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息；

其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。

可选的，所述对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，进一步包括：

对源图像帧进行图像预处理，得到目标图像帧；

基于目标图像帧进行特征提取，得到目标图像帧的特征点；

该方法进一步包括：

基于第一视觉点云地图，根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，和/或，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。

可选的，所述对源图像帧进行图像预处理，得到目标图像帧，包括：

根据相机的畸变系数，对源图像帧进行去畸变处理，得到去畸变图像；

判断去畸变图像中各个像素点的像素值是否大于第一像素阈值；如果是，则将去畸变图像中像素值大于第一像素阈值的像素点进行取反操作，然后对取反后的去畸变图像进行图像滤波，得到背景图像；否则，将去畸变图像进行图像滤波，得到背景图像；

用去畸变图像减去背景图像，得到前景图像；

判断前景图像中的像素值是否分布均匀；如果均匀，则将该前景图像作为目标图像帧；否则，对前景图像进行拉伸处理，得到目标图像帧。

可选的，所述对前景图像进行拉伸处理，包括：

若前景图像像素值小于等于设定的最小灰度值时，将该前景图像像素值取值为像素取值范围内的最小值；

若前景图像像素值大于最小灰度值、且小于设定的最大灰度值时，按照与像素最大值成一定比例的像素值作为该前景图像像素值；所述一定比例为前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值；

若前景图像像素值大于等于最大灰度值时，将该前景图像像素值取值为像素取值范围内的最大值；

所述基于目标图像帧进行特征提取，得到目标图像帧的特征点，包括：

对目标图像帧进行特征检测，得到特征点；

将目标图像帧划分成一定数量的网格；

对于任一网格中的特征点，将网格内的特征点按特征点响应值降序排列，保留前Q个特征点，得到筛选后的特征点；其中，Q根据目标图像帧中特征点的数量和设定的特征点总数上限、该网格中的特征点总数确定；

对筛选后的各特征点，分别计算特征描述符。

可选的，所述Q根据目标图像帧中特征点的数量和设定的特征点总数上限、该网格中的特征点总数确定，包括：Q为目标图像帧中特征点的数量除以设定的特征点总数上限之商，乘以网格中的特征点总数后的结果向下取整得到。

可选的，所述对源图像帧进行帧间跟踪，确定关键帧，包括：

对于每一目标图像帧：判断该目标图像帧是否为首帧；如果是，则将该目标图像帧作为关键帧；否则，根据关键帧条件确定该目标图像帧是否为关键帧；

其中，所述关键帧条件至少满足以下条件之一：

匹配特征点数量大于设定的第一阈值；

与上一关键帧之间的空间距离大于设定的第二阈值；

与上一关键帧之间的空间角度大于设定的第三阈值。

可选的，所述源图像帧为来源于单目相机、且为同一平面的图像帧；

所述计算当前关键帧中匹配特征点的空间位置信息，包括：

对于每一匹配特征点：

x坐标为：当前关键帧中该匹配特征点的像素横坐标与相机安装高度的乘积结果与相机焦距的比值；

y坐标为：当前关键帧中该匹配特征点的像素纵坐标与相机安装高度的乘积结果与相机焦距的比值；

z坐标为：相机安装高度。

可选的，所述源图像帧为来源于单目相机、且为非同一平面的图像帧；

所述计算当前关键帧中匹配特征点的空间位置信息，包括：

根据由当前关键帧中匹配特征点与上一关键帧中匹配特征点组成的至少8对匹配特征点的像素坐标，得到当前关键帧与上一关键帧的本质矩阵；

对本质矩阵进行奇异值分解，得到当前关键帧与上一关键帧之间的相对位姿；

对于每一匹配特征点：根据当前关键帧与上一关键帧之间的相对位姿，按照三角化计算关系，至少得到当前关键帧中该匹配特征点的深度值；根据当前关键帧中该匹配特征点的深度值，得到该匹配特征点的空间位置信息。

可选的，所述根据由当前关键帧中匹配特征点与上一关键帧中匹配特征点组成的至少8对匹配特征点的像素坐标，得到当前关键帧与上一关键帧的本质矩阵，包括：

对于任一匹配特征点：

根据上一关键帧中该匹配特征点的归一化平面坐标的转置矩阵、本质矩阵、当前关键帧中该匹配特征点的归一化平面坐标的矩阵之乘积等于0的关系，代入8对匹配特征点的像素坐标，得到本质矩阵；

所述根据当前关键帧与上一关键帧之间的相对位姿，按照三角化计算关系，至少得到当前关键帧中该匹配特征点的深度值，包括：

基于当前关键帧中该匹配特征点的深度值与该匹配特征点的归一化平面坐标的矩阵之乘积等于，上一关键帧中该匹配特征点的深度值、相对位姿中的旋转矩阵、以及上一关键帧中该匹配特征点的归一化平面坐标的矩阵之乘积与相对位姿中的平移矩阵之和，根据当前关键帧与上一关键帧之间的相对位姿中的旋转矩阵和平移矩阵、当前关键帧和上一关键帧中该匹配特征点的归一化平面坐标的矩阵，得到当前关键帧中该匹配特征点的深度值；

所述根据当前关键帧中该匹配特征点的深度值，得到该匹配特征点的空间位置信息，包括：

x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与该匹配特征点的深度值的乘积；

y坐标为：当前关键帧中该匹配特征点的归一化平面的像素纵坐标与该匹配特征点的深度值的乘积；

z坐标为：相机焦距。

可选的，所述源图像帧为来源于双目相机、且为非同一平面的双目图像帧；

所述对源图像帧进行图像预处理，得到目标图像帧，包括：

对来自第一目相机的第一源图像帧、来自第二目相机的第二源图像帧分别进行图像预处理，得到第一目标图像帧和第二目标图像帧，作为双目目标图像帧；

所述基于目标图像帧进行特征提取，得到目标图像帧的特征点，包括：分别提取第一目标图像帧的特征点和第二目标图像帧的特征点；

所述判断该目标图像帧是否为首帧，包括：判断双目目标图像帧是否为首帧；如果是，则将该双目目标图像帧中的任一帧作为关键帧；否则，根据关键帧条件确定该目标图像帧中的任一帧是否为关键帧；

所述计算当前关键帧中匹配特征点的空间位置信息，包括：

对于当前关键帧中每一匹配特征点：

将当前关键帧作为当前双目目标图像帧中的第一帧，将该双目目标图像帧中的另一目标图像帧作为第二帧，将第一帧中该匹配特征点与第二帧中的特征点进行匹配；如果匹配成功，得到第二帧中的匹配特征点，则：

当前关键帧中该匹配特征点的x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值；

当前关键帧中该匹配特征点的y坐标为：当前关键帧中该匹配特征点的归一化平面的像素纵坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值；

当前关键帧中该匹配特征点的z坐标为：相机焦距与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值。

可选的，所述根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，包括：

根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧；

基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束；

构造用于关键帧位姿图优化的第二目标函数，以闭环约束为约束，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧的位姿。

可选的，所述根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧，包括：

将不同次地采集到相同标识的关键帧作为闭环关键帧；

或者，

计算两关键帧间的相似度是否大于设定的相似度阈值；如果是，则判定该两关键帧为闭环关键帧，其中，相似度包括：特征点分布上的相似度和图像像素的相似度；

所述基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束，包括：

基于闭环关键帧，计算该闭环关键帧中的匹配特征点；

对于该闭环关键帧中的任一匹配特征点，根据第一闭环关键帧中该匹配特征点的像素坐标矩阵等于，第一闭环关键帧和第二闭环关键帧之间的相对位姿中的旋转矩阵与第二闭环关键帧的像素坐标矩阵的乘积加上相对位姿中的平移矩阵的关系，计算得到相对位姿，作为初始值；

构建累计闭环关键帧中的所有匹配特征点的像素位置信息误差的第一目标函数，代入所有匹配特征点的像素坐标矩阵，迭代求解使得第一目标函数取得最小值时的相对位姿；

所述构造用于关键帧位姿图优化的第二目标函数，以闭环约束为约束，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧的位姿，包括：

根据任一第一关键帧的位姿和任一第二关键帧的位姿之间的相对位姿的误差，构建累计该第一关键帧和该第二关键帧的相对位姿的误差的第二目标函数；

以该第一关键帧和第二关键帧之间的相对位姿误差作为初始值，以所述闭环约束为约束，迭代求解使得第二目标函数取得最小值时的第一关键帧的位姿和第二关键帧的位姿。

可选的，所述计算两关键帧间的相似度是否大于设定的相似度阈值，包括：

在根节点，用k均值聚类算法将所有特征点聚成k类，得到第一层节点；

对于第一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点；

对于下一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点；重复执行所述对于下一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点的步骤，直至最后的叶子层，得到视觉字典，该视觉字典包括N个特征点、且每次分叉为k的树；

其中，从根节点到叶子层共计d层，叶子层中包括视觉字典中的单词特征点；k、d、N均为自然数，N为视觉字典中特征点的总数；

对于任一关键帧，根据该关键帧中所有特征点数量以及任一单词特征点出现的次数，计算该单词特征点的权重，将该关键帧描述为以各个单词特征点及其权重为元素的集合，该集合包括有N个元素；

根据第一关键帧所描述的集合中的所有元素和第二关键帧所描述的集合中的所有元素，计算第一关键帧与第二关键帧的相似度；

若相似度大于设定的相似度阈值，判定两关键帧之间存在闭环约束。

可选的，所述根据重投影误差，对地图点的空间位置信息进行优化，包括：

对任一关键帧，根据该关键帧的位姿所采集到任一地图点在该关键帧中的像素位置所存在的重投影误差，构建重投影误差的第三目标函数；

以重投影误差初始值，迭代求解使得第三目标函数取得最小值时的地图点的空间位置信息；

其中，重投影误差初始值为：该地图点在该关键帧中的像素位置与该地图点重投影在图像中的位置之差；

所述该地图点重投影在图像中的位置根据相机内参、该关键帧的位姿、该地图点的空间位置信息得到。

本申请实施例还提供了一种视觉点云地图的构建装置，包括第一视觉点云地图构建单元，该单元包括：

特征提取模块，用于对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点；

地图点生成模块，用于对源图像帧进行帧间跟踪，确定关键帧，将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点，计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息，

本申请实施例还提供了一种电子设备，包括存储器和处理器，所述存储器存储有可执行的计算机指令，所述处理器被配置执行所述存储器中存储的指令，以实现上述任一所述视觉点云地图的构建方法的步骤。

本申请实施例还提供了一种计算机可读存储介质，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述任一所述视觉点云地图的构建方法的步骤。

本申请实施例还提供了一种计算机程序，所述计算机程序被处理器执行时实现上述任一所述视觉点云地图的构建方法的步骤。

本申请实施例提供的视觉点云地图的构建方法，通过对待建地图的空间所采集的图像帧进行特征提取，通过帧间匹配，获取匹配特征点的空间位置信息，将匹配特征点作为地图点，得到由所有关键帧的地图点集合所构成的视觉点云地图，实现了物理环境中三维点的生成和描述。

本申请实施例中，构建地图的过程将建图和定位进行分离，有效地除去了建图与定位的相互影响。在复杂多变的环境中，本申请实施例提供的地图构建方法具有更好的适应性和稳定性。

此外，由于点云地图具有连续性，相比于由地图节点所构成的地图，能实现连续的定位，避免了定位过程中的跳变问题，降低了重定位发生的概率。

进一步地，通过对地图关键帧的位姿图优化和/或地图点优化，提高了地图的精度。在发生错误闭环情况，能够及时对地图进行修正，且不会丢失初始地图数据，这使得构建地图的扩展性增强，有利于与改进的地图构建方法进行融合。

附图说明

图1为本申请实施例一提供的基于单目相机所采集的图像数据构建地图的一种流程示意图。

图2为本申请实施例提供的特征点筛选的一种示意图。

图3为本申请实施例二提供的基于单目相机所采集的前视图像数据构建地图的一种流程示意图。

图4为本申请实施例三提供的基于双目相机所采集的图像数据构建地图的一种流程示意图。

图5为本申请实施例提供的累计误差的一种示意图。

图6为本申请实施例提供的对第一视觉点云地图进行优化的一种流程示意图。

图7为本申请实施例提供的视觉字典的一种示意图。

图8为本申请实施例提供的视觉点云地图的构建装置的一种示意图。

图9为本申请实施例提供的图像预处理模块的一种示意图。

具体实施方式

为了使本申请的目的、技术手段和优点更加清楚明白，以下结合附图对本申请做进一步详细说明。

本申请实施例中，基于采集的图像数据，通过特征提取和帧间跟踪的特征点匹配，获得视觉点云地图。可选地，通过闭环约束进行位姿图优化，和/或通过重投影误差进行地图点优化，以提高地图的精度。所构建的视觉点云地图至少包括关键帧位姿信息和地图点的空间位置信息，其中，每个地图点还可以具有特征点描述符信息。

为避免定位对建图的影响，本申请实施例提供了一种视觉点云地图的构建方法，该视觉点云地图的构建方法可以应用于机器人或与机器人连接的服务器，对此不进行限定。该视觉点云地图的构建方法包括：

对源图像帧进行帧间跟踪，确定关键帧；

本申请实施例提供的技术方案中，构建地图的过程将建图和定位进行分离，有效地除去了建图与定位的相互影响。在复杂多变的环境中，本申请实施例提供的地图构建方法具有更好的适应性和稳定性。

实施例一

为便于理解，在本申请实施例中，以图像数据是由单目相机采集、图像数据为地面纹理图像为例来说明。所应理解的是，本申请实施例中，图像数据可以简称为图像或图像帧，图像帧可不限于地面纹理图像，其它类型的图像帧也可适用。

参见图1所示，图1为本申请实施例一提供的基于单目相机所采集的图像数据构建地图的一种流程示意图。该地图的构建过程可以包括以下三个阶段：图像预处理、特征提取、以及帧间跟踪。可选地，对于每一图像帧，执行如下步骤：

步骤101，将所采集的图像帧作为源图像帧，对源图像帧进行预处理，得到目标图像帧，以便于提取图像帧中的特征点。例如，视觉点云地图为地面纹理地图时，需要提取地面纹理图像帧中的纹理信息，因此，对地面纹理图像帧所进行的预处理的目的是：得到以纹理信息为主的图像帧，以便提取包括纹理信息的特征点。

可选的，上述步骤101可以细化为如下步骤：

步骤1011，根据相机的畸变系数对源图像帧进行去畸变处理，得到去畸变图像帧I(u，v)，其中，u、v表示像素坐标，I(u，v)表示去畸变图像帧中该像素坐标(u，v)处的像素值。

步骤1012，对去畸变图像帧I(u，v)进行图像滤波，得到背景图像帧I _b(u，v)。

例如，图像滤波为高斯滤波。步骤1012可以为，对去畸变图像帧I(u，v)进行高斯滤波，其中，高斯滤波核大小可以设置为45×45。

上述步骤1012可以用数学式表达为：

I _b(u，v)＝G×I(u，v)；

其中，G为图像滤波的滤波核，I _b(u，v)为背景图像帧，即滤波后的图像帧；I(u，v)为去畸变图像帧；

本申请实施例中，图像滤波核(如上述高斯滤波核)可以设置的比较大，使得滤波后的图像帧尽可能的接近真实的背景图像帧。

可选的，若纹理区域为图像帧中的较暗部分，可先将去畸变图像帧I(u，v)进行取反操作，用数学式表达为：像素最大值-I(u，v)。例如，像素最大值为255，则取反操作为：255-I(u，v)。

上述纹理区域为图像帧中特征点所在的区域。若源图像帧中纹理区域的亮度低于预设亮度阈值，则可对去畸变图像帧I(u，v)进行取反操作，得到取反后的图像帧，之后，对取反后的图像帧进行图像滤波，得到背景图像帧I _b(u，v)。

步骤1013，用去畸变图像帧减去背景图像帧，得到以纹理信息为主的前景图像帧I _f(u，v)。上述步骤1013中求得前景图像帧可以用数学式表达为：

I _f(u，v)＝I(u，v)-I _b(u，v)；

其中，I _f(u，v)为前景图像帧，I _b(u，v)为背景图像帧，即滤波后的图像帧；I(u，v)为去畸变图像帧。

步骤1014，对前景图像帧进行拉伸，得到目标图像帧。

通常情况下，采集的图像帧中纹理信息较弱，纹理区域的像素值(灰度)大多分布在狭窄的灰度区间。因此，在该步骤中，将前景图像帧的像素值拉伸到像素取值范围上，扩大前景图像帧的像素值的灰度区间。

本申请实施例中，像素取值范围可以为像素点实际能够取值的范围，即0～255。灰度值即为像素值。

一个可选的实施例中，上述步骤1014可以为：

当前景图像像素值小于等于最小灰度值时，将该前景图像像素值取值为像素取值范围内的最小值，例如，像素最小值为0；

当前景图像像素值大于最小灰度值、且小于最大灰度值时，增加该前景图像像素值的对比度。可选地，可以按照与像素最大值成一定比例的像素值作为该前景图像像素值。可选地，上述比例可以为：前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值。

当前景图像像素值大于等于最大灰度值时，将该前景图像像素值取值为像素取值范围内的最大值，例如，像素最大值为255。

上述最大灰度值和最小灰度值可以为用户根据实际需求预先设定的值。例如最大灰度值为200、220等，最小灰度值为50、100等

这种情况下，上述步骤1014可以用数学式表达为：

拉伸后的前景图像帧I _f'(u，v)表示为：

其中，I _f'(u，v)为目标图像帧，即拉伸后的前景图像帧，I _f(u，v)表示前景图像帧，I _min为最小灰度值，I _max为最大灰度值，在上式中，像素取值范围为0～255。

本申请实施例中，前景图像像素值为前景图像帧中一个像素点的像素值。对于前景图像帧中每个像素点：

当该像素点的像素值小于等于最小灰度值时，将该像素点的像素值取值为像素取值范围内的最小值；

当该像素点的像素值大于最小灰度值、且小于最大灰度值时，增加该像素点的像素值的对比度。可选的，增加对比度方式可以为：将与像素取值范围的最大值成预设比例的像素值作为该像素点的像素值。其中，预设比例可以为：第一差值和第二差值的比值，第一差值为该像素点的像素值与最小灰度值之差，第二差值为最大灰度值与最小灰度值之差。

当该像素点的像素值大于等于最大灰度值时，将该像素点的像素值取值为像素取值范围内的最大值。

图像帧预处理有利于提升图像帧的对比度，在一些纹理信息较弱的环境，有利于提高纹理区域的对比度，从而提取到更多的特征点。

步骤102，基于当前目标图像帧提取特征点，以将图像信息转换为特征信息，得到当前目标图像帧的特征点集合。

在本步骤中，可采用ORB(Oriented FAST and Rotated BRIEF，面向加速分段测试特征和二进制鲁棒独立的基本特征)、SIFT(Scale invariant feature Transform，尺度不变特征变换)、SURF(Speeded Up Robust Features，加速稳健特征)等算法提取特征点。

以ORB算法为例，基于一目标图像帧，上述步骤102可以包括：

步骤1021，基于目标图像帧，采用FAST(Features from Accelerated Segment Test，加速分段测试特征)算法进行特征检测，得到FAST特征点。

步骤1022，对FAST特征点进行筛选，以有效控制特征点的规模。

为了保证特征点分布均匀的同时，尽可能筛选出显著的特征点，一个可选的实施例中，可以将目标图像帧划分成一定数量的网格，如图2所示，图2为本申请实施例提供的特征点筛选示意图。图2中，将目标图像帧划分成多个的网格。网格的数量根据实际需求进行设定。

所有的特征点按网格进行筛选，可以为：

针对任一网格，将该网格内的特征点按FAST特征点响应值降序排列，保留前Q个特征点，其中，Q根据一目标图像帧中特征点的数量和所设的特征点总数上限、以及该网格中的特征点总数确定。不同网格所保留的特征点数可以不同，也可以相同。

例如，一目标图像帧中，特征点总数上限设定为100个，该目标图像帧中特征点的数量为2000个，则根据该目标图像帧中特征点的数量(2000个)和特征点总数上限(100个)，可以确定2000/100＝20，即该目标图像帧中每20个特征点选出一个。如果该目标图像帧的某网格中有20个特征点，则该网格保留的特征点为1，即，Q＝1。

上述Q的确定，用数学式表达可以为：

其中，符号

表示向下取整。

步骤1023，对筛选出来的每个FAST特征点，确定该FAST特征点的方向，也就是，计算特征点以r为半径范围内的质心，特征点坐标到质心形成一个向量作为该特征点的方向。

筛选出来的FAST特征点即为上述保留前Q个特征点。

上述步骤1023可以为：对筛选出来的每个FAST特征点，计算以特征点为圆心，以r为半径的范围内所有FAST特征点的质心，将该FAST特征点到质心形成一个向量作为该FAST特征点的方向。

步骤1024，对筛选出来的每个FAST特征点，计算一个二进制串的特征描述符，从而得到当前该目标图像帧中的特征点信息。

本申请实施例中，可以采用rBRIEF、oBRIEF等特征描述符表示特征点信息。

步骤103，帧间跟踪，以对前后帧中的特征点进行匹配，计算匹配特征点在世界坐标系下的坐标，作为具有三维空间位置信息的地图点保存。

上述帧间跟踪，以对前后帧中的特征点进行匹配，可以简称为帧间匹配。在世界坐标系下的坐标又可以称为空间坐标。

在该步骤103中，对当前已提取特征点的目标图像帧：

步骤1031，判断当前目标图像帧是否为首帧；如果是，则将该目标图像帧作为关键帧；否则，则执行步骤1032，进行帧间匹配，以确定当前目标图像帧是否为关键帧。

步骤1032，将当前目标图像帧与上一关键帧进行匹配，即：

对于当前目标图像帧的任一特征点i，计算当前目标图像帧中的特征点i与上一关键帧中特征点i的描述符之间的匹配度是否小于设定的匹配阈值；如果是，则判定两特征点匹配；否则，判定该两特征点不匹配。

其中，匹配度可以采用汉明距离来描述，匹配阈值为汉明距离阈值。匹配阈值大小可以根据实际需求进行设定。

当前目标图像帧中的特征点i与上一关键帧中特征点i对应空间中同一点。如果当前目标图像帧中的特征点i与上一关键帧中特征点i的描述符之间的匹配度小于设定的匹配阈值，则当前目标图像帧中的特征点i与上一关键帧中特征点i匹配，特征点i为当前目标图像帧的匹配特征点。

步骤1033，根据关键帧条件判断当前目标图像帧是否为关键帧；如果是，则将当前目标图像帧作为关键帧，执行步骤1034，以基于该关键帧进行地图更新；否则，则不进行地图更新。

在该步骤1033中，关键帧条件可以为匹配特征点数量大于设定的第一阈值。这种情况下，步骤1033可以为：当当前目标图像帧的匹配特征点数量大于设定的第一阈值时，可以判定当前目标图像帧为关键帧；

关键帧条件还可以是以下条件之一：

与上一关键帧之间的空间距离大于设定的第二阈值；

与上一关键帧之间的空间角度大于设定的第三阈值；

上述空间距离为：当前位置与上一位置之间的距离。上述空间角度为：从上一位置至当前位置，机器人所旋转的角度。当前位置为采集当前目标图像帧时机器人的位置，上一位置为采集上一关键帧时机器人的位置。

当关键帧条件为匹配特征点数量大于设定的第一阈值之外的条件时，步骤1033与步骤1032进行对调，即：先确定关键帧，然后再确定匹配特征点。

步骤1034，基于当前关键帧，计算各个匹配的特征点(简称为匹配特征点)的坐标，并作为地图点信息保存；

由于每一当前关键帧与上一关键帧的匹配特征点不完全相同，这样，上一关键帧中与当前关键帧匹配的特征点的坐标会被该步骤1034的计算结果更新，而与当前关键帧未匹配的特征点的坐标则未被更新，从而使得得到的当前地图信息包括：未更新的地图点信息和已更新的地图点信息。其中，每个地图点对应有三维空间位置信息。三维空间位置信息简称为空间位置信息，即为地图点在世界坐标系下的坐标。地图点投影在图像帧中的点即为特征点。

地图点信息可以包括空间位置信息。地图点信息还可以包括：在该地图点信息所指示的空间坐标处采集的关键帧，以及采集该关键帧时机器人的姿态。

在本申请实施例中，鉴于单目相机所采集的地面纹理图像帧处于同一平面，例如，单目相机安装于机器人底部，故而，采集图像帧时，当前关键帧的任一匹配特征点在世界坐标系下的坐标可以通过相机的外参，将当前关键帧的匹配特征点投影到图像平面上，得到图像坐标系下的像素坐标。匹配特征点在世界坐标系下的坐标即为匹配特征点的空间位置信息。

上述地面纹理图像帧处于同一平面可以理解为：各个空间点与单目相机的镜头所在平面的距离相同。空间点为地面纹理图像帧中像素点对应的世界坐标系下的点。

基于此，在采集到图像帧后，可以通过相机的外参，将当前关键帧的匹配特征点的像素坐标投影到世界坐标系下，得到匹配特征点的空间位置信息。以当前关键帧的匹配特征点i为例，上述步骤1034可以为：

x坐标为当前关键帧的匹配特征点i的像素横坐标u与相机安装高度的乘积结果与相机焦距的比值，

y坐标为当前关键帧的匹配特征点i的像素纵坐标v与相机安装高度的乘积结果与相机焦距的比值，

z坐标为相机安装高度。

可选的，匹配特征点i的空间位置信息可以用数学式表达为：

z＝H

其中，H为相机的安装高度，f为相机的焦距，u和v为匹配特征点i在图像坐标系中的像素坐标，x和y为世界坐标系下的坐标。

反复执行步骤101～103，直至所有的源图像帧处理完毕，得到由大量地图点构成的第一视觉点云地图。

反复执行步骤101～103，得到大量地图点信息，一个地图点信息可以标识出一个世界坐标系下的一个地图点。大量地图点信息可标识出大量地图点，结合关键帧，构成了第一视觉点云地图。

本申请实施例提供了一种视觉点云地图的构建方法，通过对所采集的源图像帧的后处理，以相邻关键帧进行特征点匹配，基于匹配的特征点的像素坐标生成地图点的三维坐标，从而得到视觉点云地图。本申请实施例中，在地图构建过程中，无需确定机器人的定位信息，避免了定位信息对地图构建的影响。相比于基于机器人的定位信息构建地图节点的地图构建方式，本申请实施例提供的技术方案，避免了由于地图点之间有间距而导致地图信息离散(即不连续)的问题，实现了连续地图点的构建，使得定位应用中，能实现连续的定位，无跳变问题存在。

实施例二

在本申请实施例中，以图像数据是由单目相机采集、所采集的图像帧为非同一平面的图像帧为例来说明。例如，单目相机采用前视安装，即，机器人通过前视相机采集图像帧。

上述图像帧为非同一平面可以理解为：各个空间点与单目相机的镜头所在平面的距离不同。空间点为图像帧中像素点对应的世界坐标系下的点。

参见图3所示，图3为本申请实施例二提供的基于单目相机所采集的前视图像数据构建地图的一种流程示意图。对于每一图像帧，执行如下步骤：

步骤301，根据相机的畸变系数对源图像帧进行去畸变处理，得到去畸变图像帧I(u，v)，其中，u、v表示像素坐标，I(u，v)表示图像帧中该像素坐标处的像素值。

上述步骤301中，将所采集的图像帧作为源图像帧，进而根据相机的畸变系数对源图像帧进行去畸变处理，得到去畸变图像帧I(u，v)。

步骤302，判断去畸变图像帧中各个像素点的像素值是否大于设定的第一像素阈值；如果是，则将像素值大于第一像素阈值的像素点进行取反操作，然后对进行了取反操作后的去畸变图像帧进行滤波；否则，直接对去畸变图像帧I(u，v)，进行图像滤波，得到背景图像帧I _b(u，v)。

上述步骤302可以为：对于去畸变图像帧中每个像素点，判断该像素点的像素值是否大于设定的第一像素阈值；如果大于第一像素阈值，则对该像素点进行取反操作；如果小于等于第一像素阈值，则无需对该像素点进行取反操作。在对去畸变图像帧中的所有像素点均执行了上述判断，并基于判断结果对去畸变图像帧进行处理之后，对处理之后的去畸变图像帧进行图像滤波，得到背景图像帧I _b(u，v)。

步骤303，用去畸变图像帧减去背景图像帧，得到前景图像帧I _f(u，v)。上述步骤303中求得前景图像帧可以用数学式表达为：

I _f(u，v)＝I(u，v)-I _b(u，v)。

步骤304，判断前景图像帧I _f(u，v)的像素值是否分布均匀；如果是，则将前景图像帧作为目标图像帧；否则，则对前景图像帧进行拉伸，得到目标图像帧，步骤304中的拉伸处理与步骤1014相同。

本申请实施例中，若前景图像帧中像素点的像素值在0～255区间上分布均匀，则确定该前景图像帧的图像质量较高，前景图像帧I _f(u，v)的像素值分布均匀，将该前景帧作为目标图像帧；若前景图像帧中像素点的像素值分布在一个狭窄的灰度区间上，例如前景图像帧中像素点的像素值分布在100～150区间上，则确定前景图像帧的图像质量较低，前景图像帧I _f(u，v)的像素值分布不均匀，对前景图像帧进行拉伸，得到目标图像帧。

在本步骤中，使得高图像质量不进行图像拉伸处理，而低图像质量进行图像拉伸处理，从而使得图像拉伸处理根据图像质量进行选择性处理，降低了设备的负担。

步骤305，基于当前目标图像帧提取特征点，以将图像信息转换为特征信息，得到当前目标图像帧的特征点集合。

在本步骤中，可采用ORB、SIFT、SIFT的高效改良版SURF等算法提取特征点。该步骤305与步骤102相同。

步骤306，判断当前目标图像帧是否为首帧；如果是，则将当前目标图像帧作为关键帧，然后返回步骤310；否则，则执行步骤307，进行帧间匹配，以确定当前目标图像帧是否为关键帧。

步骤307，将当前目标图像帧与上一关键帧进行匹配，即：

对于当前目标图像帧的任一特征点i，计算当前目标图像帧中的特征点i与上一关键帧中特征点i的描述符之间的匹配度是否小于设定的匹配阈值；如果是，则判定两特征点匹配，特征点i为当前目标图像帧的匹配特征点；否则，判定该两特征点不匹配。

其中，匹配度可以采用汉明距离来描述，匹配阈值为汉明距离阈值。

步骤308，根据关键帧条件判断当前目标图像帧是否为关键帧；如果是，则将当前目标图像帧作为关键帧，执行步骤309，以基于该关键帧进行地图更新；否则，则不进行地图更新，直接执行步骤310。

本申请实施例中，当满足以下关键帧条件之一时，判定当前目标图像帧为关键帧：

匹配特征点数量大于设定的第一阈值；

与上一关键帧之间的空间距离大于设定的第二阈值；

与上一关键帧之间的空间角度大于设定的第三阈值。

步骤309，基于当前关键帧，计算各个匹配特征点的坐标，并作为地图点信息保存。这样，得到的当前地图信息包括：未更新的地图点信息和已更新的地图点信息。其中，每个地图点对应有三维空间位置信息。

在本申请实施例中，鉴于单目相机所采集的图像帧处于非同一平面，故而，根据匹配特征点的像素坐标，采用八点法计算两图像帧之间的本质矩阵，对本质矩阵进行SVD(Singular Value Decomposition，奇异值分解)，得到相机相对位姿，然后基于三角化计算原理，根据两图像帧之间的相对位姿，采用最小二乘法，计算任一匹配特征点i在世界坐标系下的坐标。

上述相机相对位姿即为两图像帧之间的相对位姿。相机安装在机器人上，因此，相机相对位姿又可以理解为：机器人在采集两图像帧的位置之间的相对位姿。

可选的，步骤309中各个匹配特征点的坐标，可以按照以下步骤计算：

步骤3091，根据本质矩阵E和当前关键帧的匹配特征点i的归一化平面坐标p ₁、上一关键帧的匹配特征点i的归一化平面坐标p ₂满足：对于任一匹配特征点，上一关键帧中该匹配特征点的归一化平面坐标的转置矩阵、本质矩阵、当前关键帧中该匹配特征点的归一化平面坐标的矩阵之乘积等于0的关系，求解本质矩阵E。本质矩阵E的求解可以用数学式表达为：

其中，本质矩阵E是反映空间中一点P的像点在不同视角相机下相机坐标系中的表示之间的关系，为3×3矩阵。本质矩阵E的作用是：第一图像帧上的一个点被本质矩阵相乘，其结果为此点在第二图像帧上的对极线。

当前关键帧的匹配特征点i的归一化平面坐标

上一关键帧匹配特征点i的归一化平面坐标

p ₁和p ₂对应空间中的同一点，即p ₁和p ₂对应世界坐标系下的同一点，p ₁和p ₂为一对匹配特征点。

在本质矩阵E的求解算法中，代入八对匹配特征点的归一化平面坐标，可求解出本质矩阵。其中，当前关键帧的匹配特征点i与上一关键帧的匹配特征点i组成一对匹配特征点。

步骤3092，对本质矩阵E进行SVD，得到当前关键帧与上一关键帧之间的相对位姿，即相机的相对位姿，包括平移矩阵t和旋转矩阵R。

步骤3093，基于三角化计算原理，当前关键帧的匹配特征点i的深度值s ₁、上一关键帧的匹配特征点i的深度值s ₂满足：

s ₁p ₁＝s ₂Rp ₂+t；

采用最小二乘法，可以求解得出s ₁和s ₂。

或者，

将上式两边同时乘以p ₁的反对称矩阵p ₁^，可得：

s ₁p ₁^p ₁＝0＝s ₂p ₁^Rp ₂+p ₁^t；

由此可求得s ₂，将求得的s ₂代入式1，得到s ₁。

上式中，R表示旋转矩阵，t表示平移矩阵，p ₁为当前关键帧的匹配特征点i的归一化平面坐标，p ₂为上一关键帧匹配特征点i的归一化平面坐标。

步骤3094，根据当前关键帧的匹配特征点i的深度值s ₁，计算当前关键帧的匹配特征点i在世界坐标系下的坐标，可以为：

x坐标为：当前关键帧中该匹配特征点i的归一化平面的像素横坐标与该匹配特征点的深度值的乘积；

y坐标为：当前关键帧中该匹配特征点i的归一化平面的像素纵坐标与该匹配特征点的深度值的乘积；

z坐标为：相机焦距。

数学式表示为：

x＝s ₁u ₁；

y＝s ₁v ₁；

z＝f。

其中，f为相机焦距，以将归一化平面坐标转化为成像平面。u ₁为归一化平面坐标中的像素横坐标，v ₁为归一化平面坐标中的像素纵坐标，s ₁为当前关键帧的匹配特征点i的深度值。

步骤310，判断源图像帧是否处理完毕；如果是，则结束；否则，处理下一源图像帧，返回执行步骤301，直至所有的源图像帧处理完毕，得到由大量地图点构成的第一视觉点云地图。

本申请实施例提供了一种基于前视相机采集的非同一平面的图像帧的视觉点云地图的构建方法，通过对所采集的源图像帧的后处理，以相邻关键帧进行特征点匹配，基于匹配特征点的像素坐标生成地图点的三维坐标，从而得到视觉点云地图。本申请实施例中，在地图构建过程中，无需确定机器人的定位信息，使得地图构建与定位分离，提高了构建的地图的稳定性和对复杂环境的适应能力。

实施例三

在本申请实施例中，以图像数据是由双目相机采集为例来说明，所采集的图像帧为非同一平面的图像帧。

参见图4所示，图4为本申请实施例三提供的基于双目相机所采集的图像数据构建地图的一种流程示意图。以双目相机中的一目相机为第一目相机，双目相机中的另一目相机为第二目相机。对于每一双目图像帧，即，同一时间来自第一目相机的第一源图像帧和来自第二目相机的第二源图像帧，执行如下步骤：

步骤401，对第一源图像帧和第二源图像帧进行预处理，得到当前双目目标图像帧，包括第一目标图像帧和第二目标图像帧；。

在该步骤中，可以并行地对第一源图像帧和第二源图像帧进行预处理，也可以分别串行地对第一源图像帧和第二源图像帧进行预处理，对此不进行限定。

上述步骤401可以为：对第一源图像帧进行预处理，得到第一目标图像帧，对第二源图像帧进行预处理，得到第二目标图像帧。第一目标图像帧和第二目标图像帧构成当前双目目标图像帧。

步骤401中的预处理与步骤301～步骤304相同。

步骤402，基于当前双目目标图像帧，分别提取第一目标图像帧的特征点和第二目标图像帧的特征点，以将图像信息转换为特征信息，得到当前双目目标图像帧的特征点集合。

在本步骤中，可采用ORB、SIFT、SURF等算法提取特征点。该步骤中，第一目标图像帧和第二目标图像帧的特征点提取过程与步骤102相同。

步骤403，判断当前双目目标图像帧是否为首个双目图像帧；如果是，则将当前双目目标图像帧中的任一帧作为关键帧，执行步骤406；否则，则执行步骤404，进行帧间匹配，以确定当前双目目标图像帧中的任一帧是否为关键帧。

步骤404，为了提高匹配的效率，可将当前双目目标图像帧中的任一目标图像帧与上一关键帧进行匹配，可以得到该目标图像帧的匹配特征点，

步骤404中的匹配方式与步骤307相同。

步骤405，根据关键帧条件判断当前双目目标图像帧中的任一目标图像帧是否为关键帧；如果是，则将该目标图像帧作为当前双目目标图像帧的关键帧，执行步骤406，以基于该关键帧进行地图更新；否则，则不进行地图更新。

本申请实施例中，当满足以下关键帧条件之一时，判定该目标图像帧为关键帧：

匹配特征点数量大于设定的第一阈值；

与上一关键帧之间的空间距离大于设定的第二阈值；

与上一关键帧之间的空间角度大于设定的第三阈值。

步骤405中确定关键帧的目标图像帧与步骤404中提取匹配特征点的目标图像帧为同一目标图像帧。

步骤406，基于当前关键帧中的第一匹配特征点，搜索当前双目目标图像中匹配成功的第二匹配特征点，计算第一匹配特征点的坐标，并作为地图点信息保存。这样，得到的当前地图信息包括：未更新的地图点信息和已更新的地图点信息。其中，每个地图点对应有三维空间位置信息，即地图点坐标。

本步骤中，第二匹配特征点为当前双目目标图像帧中除当前关键帧外的一帧目标图像帧中的特征点，并且第二匹配特征点与第一匹配特征点的匹配度小于设定的匹配阈值。基于此，上述步骤406可以为：基于当前关键帧中的第一匹配特征点，搜索当前双目目标图像帧，获得与第一匹配特征点匹配的第二匹配特征点，基于第二匹配特征点计算第一匹配特征点的坐标，并第一匹配特征点的坐标作为地图点信息保存。

在本申请实施例中，当前关键帧中任一匹配特征点(第一匹配特征点)i的坐标的计算过程如下：

将当前关键帧作为当前双目目标图像帧中的第一帧，该帧中的匹配特征点已通过步骤404得到；将该双目目标图像帧中的另一目标图像帧作为第二帧；将第一帧中的匹配特征点i与第二帧中的特征点j进行匹配，即，计算匹配特征点i与特征点j的描述符之间的匹配度，基于匹配度，确定匹配特征点i与特征点j是否匹配；如果匹配，得到第二帧中的第二匹配特征点(即特征点j)，则匹配特征点i的地图点坐标为：

x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值；

y坐标为：当前关键帧中该匹配特征点的归一化平面的像素纵坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值；

z坐标为：相机焦距与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值。

数学式表示为：

其中，(u ₁，v ₁)为第一帧(即当前关键帧)的匹配特征点的归一化平面的像素坐标，(u ₂，v ₂)为第二帧的匹配特征点的归一化平面的像素坐标，f表示相机焦距，b表示双目基线长度。

上述第一帧的匹配特征点和第二帧的匹配特征点对应世界坐标系下的同一点。

如果不匹配，则放弃匹配特征点i的坐标计算。

反复执行步骤401～406，直至所有的源双目图像帧处理完毕，得到由大量地图点构成的第一视觉点云地图。

本申请实施例提供了一种基于双目图像帧的视觉点云地图的构建方法，利用双目图像帧获得匹配特征点的空间坐标，计算简单。并且，在地图构建过程中，无需确定机器人的定位信息，使得地图构建与定位分离，提高了构建的地图的稳定性和对复杂环境的适应能力。

鉴于第一视觉点云地图是通过图像帧间的连续匹配、不断记录生成的地图点而得到，基于图像帧间连续匹配的地图点生成方式会产生累计误差。随着机器人运动距离的增加，上述累计误差会越来越大。

参见图5所示，图5为本申请实施例提供的累计误差的一种示意图。图5中，T _i表示采集第i图像帧时机器人的位置，i＝0,1,…,20，左侧客观的真实轨迹，右侧为计算得到的轨迹。其中，T ₁和T ₁₉在同一位置附近，但计算得到的轨迹，因为累计误差而导致不在同一位置附近。为了消除累计误差，提高第一视觉点云地图中地图点的空间位置信息的精度，可以通过构建闭环约束，采用最小二乘法进行优化。

参见图6所示，图6为本申请实施例提供的对第一视觉点云地图进行优化的一种流程示意图。该优化方法可以包括：闭环点识别、闭环约束计算、地图优化。其中，地图优化包括位姿图优化和/或地图点优化，如下。

步骤601，通过人工标记或关键帧相似度计算，识别出第一视觉点云地图中存在闭环约束的关键帧。

实施方式之一，采用人工标记的方法：图像数据采集时在环境中布置唯一的标识图案，以使得不同次地采集到相同标识的关键帧之间产生闭环。该方式具有可靠性高的优点。

实施方式之二，自然标识的方法，即关键帧相似度计算的方法：通过计算两关键帧间的相似度是否大于设定的相似度阈值，来判断是否发生闭环。其中，相似度包括特征点分布上的相似度和图像像素的相似度。

通过关键帧相似度计算，识别出第一视觉点云地图中存在闭环约束的关键帧，可以包括：

一、构建自然特征的视觉字典。

例如，若多图像帧中共存在N个特征点，构建一个深度为d，每次分叉为k的树，则构建视觉字典的流程如下：

在根节点，用k均值聚类算法(k-means)把所有特征点聚成k类，这样得到了第一层节点。

对第一层的每个节点，把属于该节点的特征点再聚成k类，得到下一层。

依此类推，直至叶子层，其中，从根节点到叶子层共计d层。叶子层即为字典中的单词特征点。

参见图7所示，图7为本申请实施例提供的视觉字典的一种示意图。图7中，每个空心圆表示一个节点，两个节点间的连线表示查找某一特征点时的路径。

二、对于一图像帧(即关键帧)中所有特征点数量为n，单词特征点ω _i在视觉字典中出现的次数为c _i，该单词特征点ω _i的权重表示为：

根据视觉字典，任一关键帧A的描述可以为以各个单词特征点ω _i及其权重为元素的集合，数学式表达为：

A＝{(ω ₁，η ₁)，(ω ₂，η ₂)，……(ω _N，η _N)}

其中，N为视觉字典中特征点的总数。

四、根据上述视觉字典，可采用L1范数形式对任一两关键帧A和关键帧B之间的相似度S进行描述，如下：

其中，v _Ai为根据视觉字典所描述的关键帧A的集合中的元素，v _Bi为根据字典模型所描述的关键帧B的集合中的元素，N为视觉字典中特征点的总数。

若相似度S大于设定的相似度阈值，判定两帧之间存在闭环约束。

步骤602，基于判定为存在闭环约束的关键帧(后文简称为闭环关键帧)，计算闭环约束。上述步骤602可以包括：

步骤6021，基于闭环关键帧，计算该闭环关键帧中的匹配特征点：

若第一闭环关键帧A和第二闭环关键帧B存在闭环，计算第一闭环关键帧A中的任一特征点i和第二闭环关键帧B中的任一特征点j的匹配度；若匹配度小于设定的匹配度阈值，则判定该两特征点匹配，如此反复地对每个特征点进行匹配，得到m个匹配特征点。可以用数学式表达为：

P＝{p ₁，p ₂……p _m}，P′＝{p ₁′，p ₂′……p _m′}

其中，P为第一闭环关键帧A中的m个匹配特征点集合，P为第二闭环关键帧B中m个匹配特征点集合，p _i、p′ _i为像素坐标。本申请实施例中，像素坐标又可以称为像素坐标矩阵。

第一闭环关键帧和第二闭环关键帧为闭环关键帧。

上述匹配度计算可以是计算两特征点的描述符之间的汉明距离。若汉明距离小于设定的汉明阈值，则判定为两特征点匹配。

步骤6022，根据闭环关键帧中的匹配特征点，计算两闭环关键帧之间的帧间运动信息，即，计算两闭环关键帧之间的相对位姿，该相对位姿表征了累计误差。

鉴于闭环关键帧中的任一匹配特征点满足以下关系：

p _i＝Rp′ _i+t

其中，(R，t)为两闭环关键帧之间的相对位姿。(R，t)反映了两闭环关键帧之间的闭环约束，通过上述关系可计算得到相对位姿，作为初始值；i为自然数，且，1≤i≤m，p _i、p′ _i为像素坐标。

对于上述闭环约束，可采用最小二乘法求解，例如，采用非线性优化的LM(Levenberg-Marquardt，列文伯格-马夸尔特)算法求解。通过构造第一目标函数，代入闭环关键帧中所有匹配特征点的像素位置信息，迭代求解使得第一目标函数取得最小值时的ζ，从而求得R和t。可以用数学式表达为：

其中，ζ为(R，t)的李代数表示，p _i、p′ _i为像素坐标，m为闭环关键帧中匹配特征点的数量。上述像素位置信息为像素坐标。

步骤603，根据闭环约束，对第一视觉点云地图中的地图点进行优化。

对第一视觉点云地图中的地图点的优化可以包括：位姿图优化和地图点优化。其中，位姿图优化是按照步骤6031处理，地图点优化按照步骤6032处理。步骤6031与步骤6032无先后顺序。

所应理解的是，步骤6031与步骤6032还可以选择性的执行其中任一步骤的优化过程。例如，仅进行位姿图优化，或者，仅进行地图点优化。

步骤6031，鉴于任一关键帧i的位姿T _i的李代数表示ζ _i和任一关键帧j的位姿T _j的李代数表示ζ _j之间的相对位姿的误差e _ij可以表示为：

其中，符号∧表示反对称矩阵，符号∨表示反对称矩阵的逆运算，T _ij表示关键帧i和关键帧j之间的相对位姿，ζ _ij表示关键帧i和关键帧j之间的帧间相对李代数表示。其中，关键帧的位姿为采集该关键帧时相机(或机器人)的位姿。

故而，构造用于关键帧的位姿图优化的第二目标函数：

其中，Ω为误差项的权重，ε为关键帧集合，e _ij表示关键帧i和关键帧j之间的相对位姿的误差。

在第二目标函数中代入测量得到的关键帧i和关键帧j之间的相对位姿的误差，作为初始值，以步骤6022求得的闭环关键帧之间的相对位姿为约束，采用高斯-牛顿算法或LM算法，迭代求解使得第二目标函数取得最小值时的关键帧i的位姿T _i的李代数表示ζ _i和关键帧j位姿T _j的李代数表示ζ _j。

这样，根据闭环关键帧所确定的累积误差被分配至各个关键帧中，从而修正了关键帧的位姿。

步骤6032，根据任一关键帧i的位姿T _i采集到三维地图点j的坐标y _j在关键帧i中的像素位置z _ij，构造重投影误差e _ij：

其中，

表示地图点重投影在图像帧中的位置，为：

其中，I为单位矩阵，[I _3×3 0 _3×1]构成3×4的矩阵，T _i为4×4的矩阵，

为4×1的矩阵，K为相机内参。0 _3×1为[0 0 0] ^T。x为像素坐标的齐次表示，x ₁、x ₂和x ₃表示x内的三个数。

上述根据任一关键帧i的位姿T _i采集到三维地图点j的坐标y _j在关键帧i中的像素位置z _ij，构造重投影误差e _ij，可以理解为，基于关键帧i的位姿T _i，确定地图点j的坐标y _j在关键帧i中的像素位置z _ij，进而根据像素位置z _ij，构造重投影误差e _ij。其中，三维地图点j的坐标y _j为地图点j在世界坐标系下的坐标，像素位置z _ij表示地图点j在关键帧i中的像素坐标，

表示地图点j重投影在关键帧i中的像素坐标。

构造重投影误差的第三目标函数：

其中，Ω为误差项的权重，j为地图点，e _ij表示重投影误差。

在第三目标函数中代入上述根据关键帧i的位姿T _i、地图点j的坐标、相机内参、以及地图点j在关键帧i中的像素坐标所得到的重投影误差，并作为初始值，采用高斯-牛顿算法或LM算法，迭代求解使得第三目标函数取得最小值时的三维地图点j的坐标y _j，从而对地图点j的三维空间位置信息进行修正。

在该步骤6032中，可选地，关键帧i的位姿T _i可以为经过步骤6031优化后的位姿。

将优化后的关键帧的位姿和/或优化后的地图点的坐标作为视觉点云的地图信息保存。这样，通过关键帧的位姿优化和/或地图点坐标的优化，得到第二视觉点云地图。

本申请实施例中，将建图过程分离为：独立的第一视觉点云地图构建的处理阶段，以及通过闭环约束计算和地图优化，获得第二视觉点云地图的处理阶段。每一处理阶段都有对应的输出地图保存，即使发生建图不理想的情况，也保存了上一处理阶段中的原始数据。这使得构建地图的扩展性增强，方便与各种改进的地图构建方法进行融合。

参见图8所示，图8为本申请实施例提供的视觉点云地图的构建装置的一种示意图。该装置包括：第一视觉点云地图构建单元801、闭环单元802、地图优化单元803、以及用于实现地图文件的读取和保存的IO(Input Output，输入输出)单元804。

其中，来自外部的源图像帧输入至第一视觉点云地图构建单元801；第一视觉点云地图构建单元801用于生成的第一视觉点云地图；闭环单元802用于向第一视觉点云地图构建单元801生成的第一视觉点云地图添加闭环约束；地图优化单元803用于基于闭环约束对第一视觉点云地图进行关键帧位姿图优化和地图点优化。

可选地，第一视觉点云地图构建单元801可以包括：

图像预处理模块8011，用于对源图像帧进行预处理；

特征提取模块8012，用于将预处理后的源图像帧的图像信息转换为特征信息；

地图点生成模块8013，用于对源图像帧进行帧间跟踪，确定关键帧，将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点；计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息；

上述地图点生成模块8013，具体可以用于对源图像帧进行帧间跟踪，确定关键帧，计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息。

闭环单元802包括：

闭环关键帧识别模块8021，用于根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧；

闭环约束计算模块8022，用于基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束；构造用于关键帧位姿图优化的第二目标函数，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧的位姿。

地图优化单元803包括关键帧位姿图优化模块8031和/或地图点优化模块8032；

其中，关键帧位姿图优化模块8031，用于基于第一视觉点云地图，根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，得到第二视觉点云地图；

地图点优化模块8032，用于基于第一视觉点云地图，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。

参见图9所示，图9为本申请实施例提供的图像预处理模块的一种示意图。该图像预处理模块可以包括：

图像去畸变子模块，用于根据相机的畸变系数对源图像帧进行去畸变处理，得到去畸变图像帧；

图像滤波子模块，用于将去畸变图像帧进行图像滤波，得到背景图像帧；

图像差分子模块，用于用去畸变图像帧减去背景图像帧，得到前景图像帧；

图像拉伸子模块，用于对前景图像帧进行拉伸处理，得到目标图像帧。

本申请实施例中，将建图过程分离为独立的第一视觉点云地图构建单元、闭环单元和地图优化单元，各单元间无耦合关系，每一处理阶段都有对应的输出地图保存，即使发生建图不理想的情况，也保存了上一过程中的原始数据；扩展性强，方便与各种改进方法进行融合。

本申请实施例还提供了一种视觉点云地图的构建装置，该装置包括：第一视觉点云地图构建单元，该单元包括：

地图点生成模块，用于对源图像帧进行帧间跟踪，确定关键帧；将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点；计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息；

可选的，特征提取模块，具体可以用于：

对源图像帧进行图像预处理，得到目标图像帧；

基于目标图像帧进行特征提取，得到目标图像帧的特征点；

地图点生成模块，还用于基于第一视觉点云地图，根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，和/或，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。

可选的，特征提取模块，具体可以用于：

用去畸变图像减去背景图像，得到前景图像；

可选的，特征提取模块，具体可以用于：

对目标图像帧进行特征检测，得到特征点；

将目标图像帧划分成一定数量的网格；

对筛选后的各特征点，分别计算特征描述符。

可选的，地图点生成模块，具体可以用于：

其中，所述关键帧条件至少满足以下条件之一：

匹配特征点数量大于设定的第一阈值；

与上一关键帧之间的空间距离大于设定的第二阈值；

与上一关键帧之间的空间角度大于设定的第三阈值。

可选的，源图像帧为来源于单目相机、且为同一平面的图像帧；

地图点生成模块，具体可以用于：

对于每一匹配特征点：

z坐标为：相机安装高度。

地图点生成模块，具体可以用于：

可选的，地图点生成模块，具体可以用于：

对于任一匹配特征点：

此外，地图点生成模块，还具体可以用于：

这种情况下，x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与该匹配特征点的深度值的乘积；

z坐标为：相机焦距。

特征提取模块，具体可以用于：

分别提取第一目标图像帧的特征点和第二目标图像帧的特征点；

地图点生成模块，具体可以用于：判断双目目标图像帧是否为首帧；如果是，则将该双目目标图像帧中的任一帧作为关键帧；否则，根据关键帧条件确定该目标图像帧中的任一帧是否为关键帧；

对于当前关键帧中每一匹配特征点：

可选的，地图点生成模块，具体可以用于：

将不同次地采集到相同标识的关键帧作为闭环关键帧；

或者，

基于闭环关键帧，计算该闭环关键帧中的匹配特征点；

可选的，地图点生成模块，具体可以用于：

本申请实施例还提供了一种视觉点云地图的构建设备，包括存储器和处理器，存储器存储有可执行的计算机指令，处理器被配置执行存储器中存储的指令，以实现上述任一所述的视觉点云地图的构建方法的步骤。

存储器可以包括RAM(Random Access Memory，随机存取存储器)，也可以包括NVM(Non-Volatile Memory，非易失性存储器)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。

处理器可以是通用处理器，包括CPU(Central Processing Unit，中央处理器)、NP(Network Processor，网络处理器)等；还可以是DSP(Digital Signal Processing，数字信号处理器)、ASIC(Application Specific Integrated Circuit，专用集成电路)、FPGA(Field-Programmable Gate Array，现场可编程门阵列)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。

本申请实施例还提供了一种电子设备，包括存储器和处理器，存储器存储有可执行的计算机指令，处理器被配置执行存储器中存储的指令，以实现上述任一所述的视觉点云地图的构建方法的步骤。

该电子设备可以为机器人，也可以为与机器人连接的服务器。

本申请实施例还提供了一种计算机可读存储介质，该存储介质内存储有计算机程序，计算机程序被处理器执行时实现上述任一所述的视觉点云地图的构建方法的步骤。

本申请实施例还提供了一种计算机程序，所述计算机程序被处理器执行时实现上述任一所述的视觉点云地图的构建方法的步骤。

对于装置/网络侧设备/存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本申请的较佳实施例而已，并不用以限制本申请，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。

以上所述仅为本申请的较佳实施例，并不用以限制本申请，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。

Claims

一种视觉点云地图的构建方法，该方法包括，

对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点；

对源图像帧进行帧间跟踪，确定关键帧；

将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点；

计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息；

其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。
如权利要求1所述的方法，其中，所述对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点，进一步包括：

对源图像帧进行图像预处理，得到目标图像帧；

基于目标图像帧进行特征提取，得到目标图像帧的特征点；

该方法进一步包括：

基于第一视觉点云地图，根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，和/或，根据重投影误差，对地图点的空间位置信息进行优化，得到第二视觉点云地图。
如权利要求2所述的方法，其中，所述对源图像帧进行图像预处理，得到目标图像帧，包括：

根据相机的畸变系数，对源图像帧进行去畸变处理，得到去畸变图像；

判断去畸变图像中各个像素点的像素值是否大于第一像素阈值；如果是，则将去畸变图像中像素值大于第一像素阈值的像素点进行取反操作，然后对取反后的去畸变图像进行图像滤波，得到背景图像；否则，将去畸变图像进行图像滤波，得到背景图像；

用去畸变图像减去背景图像，得到前景图像；

判断前景图像中的像素值是否分布均匀；如果均匀，则将该前景图像作为目标图像帧；否则，对前景图像进行拉伸处理，得到目标图像帧。
如权利要求3所述的方法，其中，所述对前景图像进行拉伸处理，包括：

若前景图像像素值小于等于设定的最小灰度值时，将该前景图像像素值取值为像素取值范围内的最小值；

若前景图像像素值大于最小灰度值、且小于设定的最大灰度值时，按照与像素最大值成一定比例的像素值作为该前景图像像素值；所述一定比例为前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值；

若前景图像像素值大于等于最大灰度值时，将该前景图像像素值取值为像素取值范围内的最大值；

所述基于目标图像帧进行特征提取，得到目标图像帧的特征点，包括：

对目标图像帧进行特征检测，得到特征点；

将目标图像帧划分成一定数量的网格；

对于任一网格中的特征点，将网格内的特征点按特征点响应值降序排列，保留前Q个特征点，得到筛选后的特征点；其中，Q根据目标图像帧中特征点的数量和设定的特征点总数上限、该网格中的特征点总数确定；

对筛选后的各特征点，分别计算特征描述符。
如权利要求4所述的方法，其中，所述Q根据目标图像帧中特征点的数量和设定的特征点总数上限、该网格中的特征点总数确定，包括：Q为目标图像帧中特征点的数量除以设定的特征点总数上限之商，乘以网格中的特征点总数后的结果向下取整得到。
如权利要求2所述的方法，其中，所述对源图像帧进行帧间跟踪，确定关键帧，包括：

对于每一目标图像帧：判断该目标图像帧是否为首帧；如果是，则将该目标图像帧作为关键帧；否则，根据关键帧条件确定该目标图像帧是否为关键帧；

其中，所述关键帧条件至少满足以下条件之一：

匹配特征点数量大于设定的第一阈值；

与上一关键帧之间的空间距离大于设定的第二阈值；

与上一关键帧之间的空间角度大于设定的第三阈值。
如权利要求6所述的方法，其中，所述源图像帧为来源于单目相机、且为同一平面的图像帧；

所述计算当前关键帧中匹配特征点的空间位置信息，包括：

对于每一匹配特征点：

x坐标为：当前关键帧中该匹配特征点的像素横坐标与相机安装高度的乘积结果与相机焦距的比值；

y坐标为：当前关键帧中该匹配特征点的像素纵坐标与相机安装高度的乘积结果与相机焦距的比值；

z坐标为：相机安装高度。
如权利要求6所述的方法，其中，所述源图像帧为来源于单目相机、且为非同一平面的图像帧；

所述计算当前关键帧中匹配特征点的空间位置信息，包括：

根据由当前关键帧中匹配特征点与上一关键帧中匹配特征点组成的至少8对匹配特征点的像素坐标，得到当前关键帧与上一关键帧的本质矩阵；

对本质矩阵进行奇异值分解，得到当前关键帧与上一关键帧之间的相对位姿；

对于每一匹配特征点：根据当前关键帧与上一关键帧之间的相对位姿，按照三角化计算关系，至少得到当前关键帧中该匹配特征点的深度值；根据当前关键帧中该匹配特征点的深度值，得到该匹配特征点的空间位置信息。
如权利要求8所述的方法，其中，所述根据由当前关键帧中匹配特征点与上一关键帧中匹配特征点组成的至少8对匹配特征点的像素坐标，得到当前关键帧与上一关键帧的本质矩阵，包括：

对于任一匹配特征点：

根据上一关键帧中该匹配特征点的归一化平面坐标的转置矩阵、本质矩阵、当前关键帧中该匹配特征点的归一化平面坐标的矩阵之乘积等于0的关系，代入8对匹配特征点的像素坐标，得到本质矩阵；

所述根据当前关键帧与上一关键帧之间的相对位姿，按照三角化计算关系，至少得到当前关键帧中该匹配特征点的深度值，包括：

基于当前关键帧中该匹配特征点的深度值与该匹配特征点的归一化平面坐标的矩阵之乘积等于，上一关键帧中该匹配特征点的深度值、相对位姿中的旋转矩阵、以及上一关键帧中该匹配特征点的归一化平面坐标的矩阵之乘积与相对位姿中的平移矩阵之和，根据当前关键帧与上一关键帧之间的相对位姿中的旋转矩阵和平移矩阵、当前关键帧和上一关键帧中该匹配特征点的归一化平面坐标的矩阵，得到当前关键帧中该匹配特征点的深度值；

所述根据当前关键帧中该匹配特征点的深度值，得到该匹配特征点的空间位置信息，包括：

x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与该匹配特征点的深度值的乘积；

y坐标为：当前关键帧中该匹配特征点的归一化平面的像素纵坐标与该匹配特征点的深度值的乘积；

z坐标为：相机焦距。
如权利要求6所述的方法，其中，所述源图像帧为来源于双目相机、且为非同一平面的双目图像帧；

所述对源图像帧进行图像预处理，得到目标图像帧，包括：

对来自第一目相机的第一源图像帧、来自第二目相机的第二源图像帧分别进行图像预处理，得到第一目标图像帧和第二目标图像帧，作为双目目标图像帧；

所述基于目标图像帧进行特征提取，得到目标图像帧的特征点，包括：分别提取第一目标图像帧的特征点和第二目标图像帧的特征点；

所述判断该目标图像帧是否为首帧，包括：判断双目目标图像帧是否为首帧；如果是，则将该双目目标图像帧中的任一帧作为关键帧；否则，根据关键帧条件确定该目标图像帧中的任一帧是否为关键帧；

所述计算当前关键帧中匹配特征点的空间位置信息，包括：

对于当前关键帧中每一匹配特征点：

将当前关键帧作为当前双目目标图像帧中的第一帧，将该双目目标图像帧中的另一目标图像帧作为第二帧，将第一帧中该匹配特征点与第二帧中的特征点进行匹配；如果匹配成功，得到第二帧中的匹配特征点，则：

当前关键帧中该匹配特征点的x坐标为：当前关键帧中该匹配特征点的归一化平面的像素横坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值；

当前关键帧中该匹配特征点的y坐标为：当前关键帧中该匹配特征点的归一化平面的像素纵坐标与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值；

当前关键帧中该匹配特征点的z坐标为：相机焦距与双目基线长度的乘积，除以当前关键帧中该匹配特征点的归一化平面的像素横坐标与第二帧中匹配特征点的归一化平面的像素横坐标之差的绝对值。
如权利要求2至10任一所述的方法，其中，所述根据存在闭环约束的闭环关键帧，采用最小二乘法，对关键帧位姿进行图优化，包括：

根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧；

基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束；

构造用于关键帧位姿图优化的第二目标函数，以闭环约束为约束，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧的位姿。
如权利要求11所述的方法，其中，所述根据人工标记或关键帧相似度计算，识别出第一视觉点云地图中的闭环关键帧，包括：

将不同次地采集到相同标识的关键帧作为闭环关键帧；

或者，

计算两关键帧间的相似度是否大于设定的相似度阈值；如果是，则判定该两关键帧为闭环关键帧，其中，相似度包括：特征点分布上的相似度和图像像素的相似度；

所述基于闭环关键帧，计算闭环关键帧之间的相对位姿，作为闭环约束，包括：

基于闭环关键帧，计算该闭环关键帧中的匹配特征点；

对于该闭环关键帧中的任一匹配特征点，根据第一闭环关键帧中该匹配特征点的像素坐标矩阵等于，第一闭环关键帧和第二闭环关键帧之间的相对位姿中的旋转矩阵与第二闭环关键帧的像素坐标矩阵的乘积加上相对位姿中的平移矩阵的关系，计算得到相对位姿，作为初始值；

构建累计闭环关键帧中的所有匹配特征点的像素位置信息误差的第一目标函数，代入所有匹配特征点的像素坐标矩阵，迭代求解使得第一目标函数取得最小值时的相对位姿；

所述构造用于关键帧位姿图优化的第二目标函数，以闭环约束为约束，采用最小二乘法，求解使得第二目标函数取得最小值时的关键帧的位姿，包括：

根据任一第一关键帧的位姿和任一第二关键帧的位姿之间的相对位姿的误差，构建累计该第一关键帧和该第二关键帧的相对位姿的误差的第二目标函数；

以该第一关键帧和第二关键帧之间的相对位姿误差作为初始值，以所述闭环约束为约束，迭代求解使得第二目标函数取得最小值时的第一关键帧的位姿和第二关键帧的位姿。
如权利要求12所述的方法，其中，所述计算两关键帧间的相似度是否大于设定的相似度阈值，包括：

在根节点，用k均值聚类算法将所有特征点聚成k类，得到第一层节点；

对于第一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点；

对于下一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点；重复执行所述对于下一层的每个节点，将属于该节点的特征点聚成k类，得到下一层节点的步骤，直至最后的叶子层，得到视觉字典，该视觉字典包括N个特征点、且每次分叉为k的树；

其中，从根节点到叶子层共计d层，叶子层中包括视觉字典中的单词特征点；k、d、N均为自然数，N为视觉字典中特征点的总数；

对于任一关键帧，根据该关键帧中所有特征点数量以及任一单词特征点出现的次数，计算该单词特征点的权重，将该关键帧描述为以各个单词特征点及其权重为元素的集合，该集合包括有N个元素；

根据第一关键帧所描述的集合中的所有元素和第二关键帧所描述的集合中的所有元素，计算第一关键帧与第二关键帧的相似度；

若相似度大于设定的相似度阈值，判定两关键帧之间存在闭环约束。
如权利要求2至10任一所述的方法，其中，所述根据重投影误差，对地图点的空间位置信息进行优化，包括：

对任一关键帧，根据该关键帧的位姿所采集到任一地图点在该关键帧中的像素位置所存在的重投影误差，构建重投影误差的第三目标函数；

以重投影误差初始值，迭代求解使得第三目标函数取得最小值时的地图点的空间位置信息；

其中，重投影误差初始值为：该地图点在该关键帧中的像素位置与该地图点重投影在图像中的位置之差；

所述该地图点重投影在图像中的位置根据相机内参、该关键帧的位姿、该地图点的空间位置信息得到。
一种视觉点云地图的构建装置，其中，该装置包括第一视觉点云地图构建单元，该单元包括：

特征提取模块，用于对待建地图的空间所采集的源图像帧，进行特征提取，得到源图像帧特征点；

地图点生成模块，用于对源图像帧进行帧间跟踪，确定关键帧；将当前关键帧中的特征点与上一关键帧中的特征点进行匹配，得到当前关键帧的匹配特征点；计算当前关键帧中匹配特征点的空间位置信息，将匹配特征点的空间位置信息作为当前关键帧的地图点信息；

其中，所有关键帧的地图点集合所构成的点云为第一视觉点云地图。
一种电子设备，其中，包括存储器和处理器，所述存储器存储有可执行的计算机指令，所述处理器被配置执行所述存储器中存储的指令，以实现如权利要求1至14任一所述视觉点云地图的构建方法的步骤。
一种计算机可读存储介质，其中，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至14任一所述视觉点云地图的构建方法的步骤。
一种计算机程序，其中，所述计算机程序被处理器执行时实现如权利要求1至14任一所述视觉点云地图的构建方法的步骤。