CN112507056A - 一种基于视觉语义信息的地图构建方法 - Google Patents

Abstract

本发明公开了一种基于视觉语义信息的地图构建方法，包括以下步骤：S1、利用神经网络提取图像的语义信息；S2、结合深度图像和摄像机内的参数，将语义像素投影到三维空间，并且进行聚类处理，得到语义超点云，所述语义超点云属于局部坐标系系统；S3、地图增量式构建：根据摄像机外参数，将第一帧作为全局坐标系，将所有局部坐标系转换到全局坐标系，并对重叠的超点云进行加权合并；S4、对于若干个局部地图，如果能找到相对位置关系的，根据相对位置关系进行步骤S3；找不到相对位置关系的，选择其中的一个作为全局坐标系，利用超点云计算其他地图到全局地图的匹配关系以得到相对位置关系，再进行步骤S3。

Description

一种基于视觉语义信息的地图构建方法

技术领域

本发明涉及机器人视觉SLAM领域，特指一种基于视觉语义信息的地图构建方法。

背景技术

机器人SLAM(Simultaneously Localization And Mapping)技术，指机器人通过自身携带的传感器感知外部信息，并同时进行定位和建立地图的过程。传统的视觉SLAM技术使用摄像头作为传感器，利用图像特征点(如ORB，SIFT等等)计算图像之间的匹配关系，并通过将这些特征点投影到三维空间建立地图。这种方法的效果取决于特征点之间能否建立起正确的匹配关系。由于所有的特征点都是通过图像特征计算的，因此传统的SLAM往往假设，在一次SLAM过程中视频的光照条件不变或变化有规律。在室内环境下，这种假设相对容易达成；在室外环境下，由于太阳光的影响，传统的SLAM难以应对突然的光照变化导致像素特征的剧变。另外，变化的像素特征也使得在不同光照条件下建立的地图难以相互融合，导致了地图无法重复利用。

目前，绝大多数成熟的开源SLAM算法[1][2]使用基于特征点词袋模型[3]的方法进行回环检测和重定位。这种方法利用图像特征点(如ORB，SIFT等等)作为描述图像的词袋(Bag of Words)，并通过检索词袋之间的相似度作为回环检测的方法。但是，由于光照变化带来的图像亮度变化将影响特征点的描述子，使得理应匹配的两点的相似度大幅度下降，导致错误匹配。对此，也有一些方法(常思雨.基于视觉SLAM的语义地图研究[D].哈尔滨工业大学,2018.)试图利用语义信息改善建图效果，但只是将语义信息附加到地图上，而没有提高地图的可用性。目前有利用语义信息作为描述子来表示图像，并以此计算相似度来进行回环检测，但是仍然局限于基于图像匹配的思路。

发明内容：

针对传统的SLAM技术在建立地图上存在的问题，本发明提出一种基于视觉语义信息的地图定义、构建、融合方法，建立的地图通过空间匹配进行重定位，利用语义信息不受光照条件变化的特点克服了特征点方法的缺点，并且大大降低了地图的体积。

本发明至少通过如下技术方案之一实现。

一种基于视觉语义信息的地图构建方法，包括以下步骤：

S1、语义信息提取：利用神经网络提取图像的语义信息；

S2、语义超点云拟合：结合深度图像和摄像机内的参数，将语义像素投影到三维空间，并且进行聚类处理，得到语义超点云，所述语义超点云属于局部坐标系系统；

S3、地图增量式构建：根据摄像机外参数，将第一帧作为全局坐标系，将所有局部坐标系转换到全局坐标系，并对重叠的超点云进行加权合并；

S4、多地图融合：对于若干个局部地图，如果能找到相对位置关系的，根据相对位置关系进行步骤S3；找不到相对位置关系的，选择其中的一个作为全局坐标系，利用超点云计算其他地图到全局地图的匹配关系以得到相对位置关系，再进行步骤S3。

优选的，所述步骤S1具体包括：

对于一帧RGB图像用F_i表示，其中包含nm个像素点p₁,p₂...p_nm，p_j＝(r_j,g_j,b_j)，分别表示该像素点的R、G、B分量，nm分别为该图像的高度和宽度，则神经网络处理过程表示为：

F_i'＝N(F_i) (1)

其中N表示语义分割神经网络处理过程，其接收一个图像作为输入，返回一个图像作为输出；F_i'表示经由神经网络处理后的图像，其中包含nm个像素点p′₁，p′₂...p′_nm，p′_j＝(l_j)，l_j表示该点的类别，l_j∈(L₁,L₂...L_n)表示l_j可能的取值，这是由神经网络的属性决定的。

优选的，步骤S2所述的投影具体包括：

令在图像中的任一像素点O，该点投影到摄像机坐标系内的点P，则根据小孔透视成像模型，得：

其中(u,v,1)^T是O点在图像内的像素位置，(X,Y,Z)^T是P点在三维空间中的位置，K是相机内参，其中c_x、c_y分别表示光心在像素平面上的位置，f_x、f_y分别表示单位长度能表达的像素数；

对所有经由所述神经网络处理的图像进行投影，点云的数量为nm，取其中的一个点p_i＝(x_i,y_i,z_i,l_i)代表p_i在摄像机坐标系中X、Y、Z轴的分量以及所属的类别，l_j∈(L₁,L₂...L_n)代表所有可能的类型。

优选的，所述聚类处理是根据点p_i的类别l_j的属性，选择以下步骤中的一个进行：

(1)对于一些适合用墙体进行拟合的物体，使用平面对所有符合的点云进行拟合，考虑属于类型l_k的点云p₁,p₂...p_n，使用平面对这些点云进行最小二乘拟合，即找到一个平面F＝(a,b,c,d)：

ax+by+cz+d＝0 (3)

其中a,b,c,d分别为定义该平面的参数，x,y,z分别为该平面的变量，满足待求解的最小二乘函数为：

在找到该平面后，规定a、b、c、d的符号必须满足以下特性：

ax₀+by₀+cz₀+d＞0 (5)

其中x₀,y₀,z₀分别代表摄像机的光心位置，即摄像机坐标系原点必须处于所有平面的法向量正方向，这个特性将在步骤S4中进行超点云匹配；

(2)对于一些适合用球体进行拟合的物体，使用椭球对点云进行拟合，考虑属于类型l_k的点云p₁,p₂...p_n，使用椭球对其进行拟合，假设一个椭球用s＝(x₀,y₀,z₀,A,B,C)表示，即：

其中x₀,y₀,z₀表示椭球的球心，A,B,C分别是在x,y,z轴分量的参数，满足待求解的最小二乘函数为：

优选的，所述步骤S3具体包括：

将超点云转换到统一坐标系，定义误差函数F(x₁,x₂)计算两个超点云之间的相似度：

其中x₁、x₂分别代表两个用于比较的超点云，l₁、l₂分别是、x₂的类别，仅在两个超点云的类别相同时计算相似度，D代表两个超点云在同一个坐标系下的欧氏距离，G(x₁,x₂)为两个椭球的相似度，k₁...k₅分别是不同的系数，a₁、b₁、c₁与a₂、b₂、c₂分别是两个超点云所属椭球的参数；为了加快匹配速度，利用kd-tree组织超点云以加快查找速度；

如果一个超点云找到一个匹配，即该超点云与所有其他超点云计算相似度得到的最大值大于阈值k_corr，那么将这个超点云进行合并，如果一个超点云找不到一个匹配，即该超点云与所有其他超点云计算相似度得到的最大值小于阈值k_corr，那么将该超点云直接在全局空间坐标系中添加。

优选的，所述步骤S4具体包括：

假定有若干个地图M1,M2...Mn，固定M1坐标系为全局坐标系，将其他地图转换到该坐标系，然后进行超点云合并；多地图融合分为两种情况：

a)各地图之间有相对位置关系的情况：对于有各地图之间的相对位置关系的情况，则直接对除了地图M1以外的地图Mn中的每一帧执行步骤S3，即地图增量式构建，在地图M1的坐标系即全局坐标系中插入、合并所有地图中的超点云；

b)对于各地图之间没有相对位置关系的情况，需要首先利用超点云建立匹配关系。

优选的，建立超点云之间的匹配关系如下：

假设用于查询匹配关系的帧F'来自局部地图坐标系A，目的地图坐标系B包含帧F₁,F₂...F_n,首先利用词向量(BoW,Bag of Words)相关度建立初步匹配关系，假设所述神经网络提供的语义信息共有n类，即l_i∈(L₁,L₂...L_n)，则建立有关帧F的词向量v，v是一个n维向量，在第i维上的值表示在该帧F的周围k米内存在l＝L_i的超点云的个数，对于两帧F₁、F₂,根据两帧F₁、F₂的词向量计算相似度：

根据词向量相似度比较，在目的地图坐标系中找到帧F_k，F_k与待查询匹配关系帧F'相似度最高，因此，在F_k的周围的超点云中查找匹配关系；

在利用词向量相似度进行初步过滤之后，使用随机采样一致性(RANSAC)框架得到点云之间的匹配。

优选的，使用随机采样一致性(RANSAC)框架得到点云之间的匹配关系，具体步骤如下：

1)对查询帧F中的相同类型的点云，随机赋予一个匹配关系；

2)在所有的匹配关系中，随机选择m个匹配关系并去除；

3)对剩下的匹配关系，利用基于SVD的ICP匹配求解方法计算转移矩阵T，并计算在T矩阵变换后，匹配的点云之间的欧式距离误差；

4)如果新的匹配关系带来的误差更小，那么记录这个匹配关系；

5)重复n次后，以匹配误差最小的T矩阵为最终的匹配关系，并利用平面语义来排除错误的匹配关系。

优选的，步骤5)利用平面语义来排除错误的匹配关系具体如下：

假设拍摄帧F的摄像机坐标系原点在T矩阵变换下变换到世界坐标系p_world＝(x,y,z,1)，验证的平面为f＝(a,b,c,d)，如果：

ax+by+cz+d＜0 (10)

则表示该点云位于平面的后方，则需要抛弃该结果，找到T矩阵之后，为了排除误差，在RANSAC框架下寻找局部地图坐标系A与目的地图坐标系B之间的相对位置关系。

优选的，寻找局部地图坐标系A与目的地图坐标系B之间的相对位置关系包括以下步骤：

(S1)对地图A中的每一帧，计算得到一个到达地图B的转移矩阵T，记为T₁,T₂...T_n；

(S2)对T₁,T₂...T_n中，随机认为其中k个矩阵正确，并用最小二乘法计算得到一个临时转移矩阵T_temp作为此次迭代选择的转移矩阵；

(S3)根据T_temp将地图B中的超点云

转换到地图A的坐标系，记为

对每一个超点云计算误差大小并求和作为整体误差，超点云的误差定义为

与其在地图A中的匹配超点云

之间的欧氏距离；

(S4)如果来自步骤S2、步骤S3的新的选择T_temp带来的整体误差更小，记录这个选择；

(S4)迭代M次数后，以误差最小的选择作为最终结果；

最终，得到地图A与地图B之间的相对位置关系，即T，根据该相对位置关系，执行步骤S3实现两个地图的合并。

本发明相对于现有的技术，具有以下的优点：

利用语义分割不因光照条件的变化而改变结果的特性，使得在不同光照条件下建立的地图也可以融合，大大增强了地图的鲁棒性。同时，利用语义分割去除了移动物体，可以避免这些物体对地图造成干扰。另外，使用更高层次的语义信息大大降低了地图的占用空间大小，提高了利用效率。语义利用语义信息作为数据关联的重要参数，相比传统的点云拼接具有更好的伸缩性。

附图说明

图1本实施例摄像机小孔成像模型示意图；

图2本实施例待处理的图像；

图3本实施例语义分割建筑物的结果图；

图4本实施例语义像素投影结果图；

图5本实施例一种基于视觉语义信息的地图构建方法获得的语义地图。

具体实施方式：

下面将结合实施例对发明的步骤做进一步的说明。需要说明的是，这里描述的仅仅是本发明的一部分实施例，本发明的实施方式不仅仅限于此实施例。

本实施例的一种基于视觉语义信息的地图构建方法，包括以下步骤：

S1、语义信息提取：利用神经网络提取图像的语义信息，具体包括：

对于一帧RGB图像用F_i表示，其中包含nm个像素点p₁,p₂...p_nm(nm分别为该图像的高度和宽度)，p_j＝(r_j,g_j,b_j)，分别表示该像素点的R、G、B分量，则神经网络处理过程表示为：

F_i'＝N(F_i) (1)

其中N表示语义分割神经网络处理过程，其接收一个图像作为输入，返回一个图像作为输出。F_i'表示经由神经网络处理后的图像，其中包含nm个像素点p′₁，p'₂...p'_nm，p'_j＝(l_j)，l_j表示该点的类别，l_j∈(L₁,L₂...L_n)表示l_j可能的取值，这是由神经网络的属性决定的。

本实施例利用语义分割神经网络对RGB图像进行处理。根据神经网络训练时的预设值，可以输出一张和输入图片相同尺寸的图片，其中每一个像素用其类别(label)而不是颜色表示。这样，就可以用类别来描述这张图像。本发明不涉及神经网络的训练过程，仅使用神经网络作为进行语义分割的工具。

设一次视频序列包含图像帧F₁,F₂...F_n,图2是其中的一张图像F_k。设K为已知的摄像机的内参矩阵。利用语义分割神经网络对其进行处理，图3是对建筑物语义进行分割的结果。

S2、语义超点云拟合：结合深度图像和摄像机内的参数，将语义像素投影到三维空间，并且进行聚类处理，得到语义超点云，所述语义超点云属于局部坐标系系统；

如图1所示，利用小孔成像模型将语义分割的结果投影到三维空间。根据惯例，定义摄像机坐标系O_c，其原点位于图像中央位置，z轴垂直指向图像内部，x轴和y轴分别平行图像的宽边和高边，按照右手坐标系分布，利用下列公式将语义像素从二维平面投影到三维空间：

其中(u,v,1)^T是O点在图像内的像素位置，(X,Y,Z)^T是P点在三维空间中的位置，K是相机内参，其中c_x、c_y分别表示光心在像素平面上的位置，f_x、f_y分别表示单位长度能表达的像素数；本实施例中Z是已知量，来自深度相机提供的深度图像，X,Y是未知量，根据上式计算可以得出。对所有经由所述神经网络处理的图像进行投影，点云的数量为nm，取其中的一个点p_i＝(x_i,y_i,z_i,l_i),分别代表p_i在摄像机坐标系中X、Y、Z轴的分量以及所属的类别，l_j∈(L₁,L₂...L_n)代表所有可能的类型；图4是将点云投影到三维空间的结果。

在三维空间中，根据不同语义类型对点云实施聚类，根据点p_i的类别l_j的属性，选择以下步骤中的一个进行：

(1)对于一些适合用墙体进行拟合的物体(如墙体，地面等)，使用平面对所有符合的点云进行拟合。考虑属于类型l_k的点云p₁,p₂...p_n，使用平面对这些点云进行最小二乘拟合，即找到一个平面F＝(a,b,c,d)：

ax+by+cz+d＝0 (3)

其中a,b,c,d为代表该平面的四个参数，x,y,z是自变量。待求解的最小二乘函数为：

由于这个方程是一个凸函数，可以通过代数方法直接进行求解。便利起见，假设平面方程为：

z＝a₀x+a₁y+a₂ (5)

其中a₀,a₁,a₂分别为代表该平面的参数，x,y,z是自变量。由最小二乘法可知，目的是求解：

即找到一个参数集a₀,a₁,a₂，使得该式最小。其中x_i,y_i,z_i表示用于拟合该平面的点云的三维空间坐标值，n为所有用于拟合该平面的点云的个数。

对该式分别令a₀、a₁、a₂的偏导数为零：

将上式变换可得：

将其转换为矩阵形式为：

求解该式即可得到平面方程的各项参数。便利起见，依旧使用F＝(a,b,c,d)表示平面，参数转换公式为：

以上仅考虑所有从属于该类别的点云都来自同一类别平面的投影。实际情况中会出现多个平面同时存在的情况。这种情况下，需要计算这些点云的法向量，并首先利用法向量对点云进行聚类，将法向量相似且连续的点云归为一类，然后使用上述最小二乘方法进行平面拟合。

需要注意的是，虽然可以同时改变a、b、c、d的符号且表达同一个平面，但规定a、b、c、d的符号必须满足以下特性：

ax+by+cz+d＞0 (11)

即摄像机坐标系原点必须处于所有平面的法向量为正方向。这个特性将在步骤S4中帮助进行超点云匹配。

(2)对于一些适合用球体进行拟合的物体(如树木，电线杆等)，使用椭球对点云进行拟合。考虑属于类型l_k的点云p₁,p₂...p_n，使用椭球对其进行拟合。假设一个椭球可以用s＝(x₀,y₀,z₀,A,B,C)表示，即：

其中x₀,y₀,z₀表示椭球的球心，A,B,C分别是在x,y,z轴分量的参数。满足待求解的最小二乘函数为：

同样使用代数方法进行求解，将椭球公式写成一般式：

其中，a、b、c、d、e、f为中间变量；

重写待求解的最小二乘函数为：

对该式分别令a,b,c,d,e,f的偏导数为零，并将其整理成矩阵形式，则有：

求解该式，再将a,b,c,d,e,f带入到椭球方程，即可得到椭球公式。

上式考虑的是单一椭球的情况。实际情况中对于某一特定类型一般存在多余一个物体，即需要用多个椭球进行拟合，这时需要利用k聚类方法对点云进行分割。在分割完点云之后，再利用椭球公式进行拟合。为了加快计算速度，也可以使用k聚类中每一类的中心，方差等统计信息直接进行拟合。

在这里设定k值为0.95，即当点云N中有0.95N个点满足上式则不再重复迭代。考虑到可能存在多个物体的情况，首先利用点云相关处理方法，去除那些离散的被认为是界外点的点云，之后对三维空间中相同类别的点云进行自适应k聚类。得到聚类结果后，再利用上述公式使用椭球进行拟合。

完成步骤S2后，得到一系列平面f₁,f₂...f_n，其中f_i＝(a_i,b_i,c_i,d_i,l_i)，a_i,b_i,c_i,d_i代表该平面公式，l_i代表该平面从属的语义类型。同时得到一系列超点云P₁,P₂...P_n,其中P_i＝(x_i,y_i,z_i,l_i)，x_i,y_i,z_i代表该超点云在全局地图坐标系中的位置，l_i代表该平面从属的语义类型。

S3、地图增量式构建：根据摄像机外参数，将第一帧作为全局坐标系，将所有局部坐标系转换到全局坐标系，并对重叠的超点云进行加权合并，具体为：

假设第一个视频帧的局部空间坐标系为全局空间坐标系，即后面的所有帧全部统一到第一个视频帧的坐标系中。假设前一帧到全局空间坐标系的转移矩阵为T，局部空间坐标系中的超点云p，在全局空间坐标系中的对应位置p'，则有：

p'＝Tp (17)

利用上式将局部空间坐标系中的超点云转换到全局空间坐标系。

利用误差函数F(x₁,x₂)用来计算两个超点云之间的相似度：

其中x₁,x₂分别代表两个用于比较的超点云，l₁,l₂分别是x₁,x₂的类别，仅在两个超点云的类别相同时计算相似度。D代表两个超点云在同一个坐标系下的欧氏距离，G(x₁,x₂)则计算了两个椭球的相似度，k₁...k₅分别是不同的系数。特别地，由于在地图增量式构建过程中，预先知道点云的分布情况，因此可以直接建立kd-tree来进行范围搜索，与待匹配超点云距离小于阈值D_k，避免对整个点云进行遍历。

如果一个超点云可以找到一个匹配，即它与所有其他超点云计算相似度得到的最大值大于阈值k_corr，那么就可以将这个超点云进行合并。如果一个超点云找不到一个匹配，即即它与所有其他超点云计算相似度得到的最大值小于阈值k_corr，那么就将该超点云直接在全局空间坐标系中添加。

步骤S3是针对单个视频序列进行的。对于一系列视频帧，首先对前两帧进行S1、S2步骤，然后进行S3步骤进行超点云融合。然后，将融合后的结果(称之为全局超点云)与第3帧融合，直至对所有视频帧处理完毕。在这个过程中全局超点云随着处理的帧的数量的增加而不断增长，因此称之为增量式地图构建。

S4、多地图融合：对于若干个局部地图，如果能找到相对位置关系的，根据相对位置关系进行步骤S3；找不到相对位置关系的，选择其中的一个作为全局坐标系，利用超点云计算其他地图到全局地图的匹配关系以得到相对位置关系，再进行步骤S3。一个视频序列将建立一个局部地图，而多个地图之间也可以融合成为一个全局地图。假定多个地图M1,M2...Mn，固定M1坐标系为全局坐标系，将其他地图转换到该坐标系，然后进行超点云合并。

多地图融合分为两种情况：有无各地图之间的相对位置关系。对于有各地图之间的相对位置关系的情况，多地图融合较为简单：对除了地图M1以外的地图Mn中的每一帧执行步骤S3，即地图增量式构建，就可以在地图M1的坐标系(即全局坐标系)中插入、合并所有地图中的超点云。对于各地图之间没有相对位置关系的情况，需要利用超点云建立匹配关系，求解各地图之间的相对位置关系，然后按照步骤S3建立全局地图。

为了利用超点云匹配建立各个局部地图之间的匹配关系，首先描述如何根据匹配关系求解两个超点云之间的转移矩阵(transformation matrix)。假设地图A的点云A包含点(p₁,p₂...p_n)，地图B的点云B包含(p₁',p'₂...p'_n)，其中p_i和p_i'是对应匹配的点，即可以通过一个转移矩阵T，使得：

p′_i＝Tp_i (19)

目的是求解T，这是一个3D-3D姿态估计问题(或称点云配准问题)，在这里不加证明地描述如何求解转移矩阵T：H矩阵为计算的中间变量；

H＝UΣV^T (21)

R＝VU^T (22)

其中R为T的旋转分量，t为T的位移分量，将H进行SVD分解后，利用U和V矩阵计算得到R和t。

上式说明在理想情况下(即每一个点都找到了正确的匹配)如何通过SVD求解的方式解决点云配准问题。在存在误匹配的情况下，上式也可以计算得到一个最小二乘解。当所有点处于同一个平面或点的数目过少，可能导致求出的R有误。不过，在点云的数量大于10的情况下这种情况几乎不会发生。

由于点云处于不同的坐标系，一旦通过点云配准建立了两个坐标系之间的位置关系，就可以进行多地图融合。

建立超点云之间的匹配关系：假设用于查询匹配关系的帧F'来自局部地图坐标系A，目的地图坐标系B包含帧F₁,F₂...F_n,可以首先利用词向量(BoW,Bag of Words)相关度建立初步匹配关系。假设神经网络提供的语义信息共有n类，即l_i∈(L₁,L₂...L_n)，则可以建立有关帧F的词向量v，v是一个n维向量，在第i维上的值表示在该帧F的周围k米内存在l＝L_i的超点云的个数。对于两帧F₁,F₂,可以根据他们的词向量计算相似度：

根据词向量相似度比较，可以在目的地图坐标系中找到帧F_k，其与待查询匹配关系帧F'相似度最高。因此，在F_k的周围的超点云中查找匹配关系，以减少不必要的计算。

在利用词向量相似度进行初步过滤之后，接下来考虑如何建立查询帧F的点云和以相似帧F_k为中心，半径k范围内的点云之间的匹配关系。使用随机采样一致性(RANSAC)框架指导点云之间的匹配，步骤如下：

(1)对查询帧F中的相同类型的点云，随机赋予一个匹配关系。

(2)在所有的匹配关系中，随机选择m个匹配关系并去除。

(3)对剩下的匹配关系，利用基于SVD的ICP匹配求解方法计算转移矩阵T，并计算在T矩阵变换后，匹配的点云之间的欧式距离误差。

(4)如果新的匹配关系带来的误差更小，那么记录这个匹配关系。

(5)重复n次后，以匹配误差最小的T矩阵为最终结果。

前语义超点云根据其类别，分别以椭球和平面进行拟合。对于被定义为平面的那些超点云，不使用其作为匹配，而是利用平面来排除错误的匹配关系。假设拍摄帧F的摄像机坐标系原点在T矩阵变换下变换到世界坐标系p_world＝(x,y,z,1)，验证的平面为f＝(a,b,c,d)如果：

ax+by+cz+d＜0 (26)

则表示该点位于平面的后方，则需要抛弃该结果，因为默认墙面的后方是实体，摄像机不可能出现在该位置。

RANSAC框架在概率学上保证可以找到一个近似解，而且迭代的次数越多则找到解的概率越大。由于之前已经通过词向量相似度匹配进行了粗略估计，这里的迭代次数不需要设的特别大。

通过RANSAC框架找到T矩阵之后，理论上就可以通过该矩阵建立地图A和地图B之间的联系。但是考虑到词向量相似度可能存在错误匹配的情况，因此同样在RANSAC框架下寻找两个地图之间的相对位置关系，步骤如下：

(1)对地图A中的每一帧，计算得到一个到达地图B的转移矩阵T，记为T₁,T₂...T_n.

(2)对T₁,T₂...T_n中，随机认为其中k个矩阵正确，并根据地图A原有位置关系计算误差大小。

(3)如果新的选择带来的误差更小，记录这个选择。

(4)迭代一定次数后，以误差最小的选择作为最终结果。

需要注意的是，由于地图A和地图B并非完全重合，有一部分来自A的查询帧将不会得到可靠的解T，这一部分解在上述的RANSAC步骤中被去除了。

最终，得到地图A与地图B之间的相对位置关系，即T。根据该相对位置关系，执行步骤S3实现两个地图的合并。对于多个地图，则依次执行步骤S4，最终将所有局部地图统一到全局坐标系。

以两个地图A、B为例，假设A、B地图的相对位置关系未知，需要找到一个坐标变换矩阵T使得在B坐标系中的超点云p，在全局空间坐标系(即A坐标系)中的对应点云p'，满足：

p'＝Tp (27)

即利用在两个地图坐标系中同时出现的超点云建立两个地图坐标系之间的坐标变换矩阵。在找到该矩阵T之后，按照步骤S3在全局地图进行地图融合。

建立超点云之间的匹配关系：假设用于查询匹配关系的帧F'来自局部地图坐标系A，目的地图坐标系B包含帧F₁,F₂...F_n,可以首先利用词向量相关度建立初步匹配关系。对于两帧F₁,F₂,可以根据他们的词向量计算相似度：

根据词向量相似度比较，可以在目的地图坐标系中找到帧F_k，其与待查询匹配关系帧F'相似度最高。因此，在F_k的周围的超点云中查找匹配关系，以减少不必要的计算。

利用RANSAC方法，在待查询帧F'的超点云中和最匹配帧F_k的周围的超点云中查找匹配关系。然后利用平面来排除错误的匹配关系，假设拍摄帧F的摄像机坐标系原点在T矩阵变换下变换到世界坐标系p_world＝(x,y,z,1)，验证的平面为f＝(a,b,c,d)如果：

ax+by+cz+d＜0 (29)

则表示该点位于平面的后方，则需要抛弃该结果，因为默认墙面的后方是实体，摄像机不可能出现在该位置。

通过RANSAC方法找到T矩阵之后，就可以通过该矩阵建立地图A和地图B之间的联系。但是考虑到词向量相似度可能存在错误匹配的情况，因此同样在RANSAC框架下寻找两个地图之间的相对位置关系关系。

对于B地图中的帧F₁,F₂...F_n,分别求解得到坐标变换矩阵T₁,T₂...T_n，其中只有一部分是正确的。同样使用RANSAC方法排除其他错误的解，最终得到转移矩阵T，它是两个地图之间的相对位置关系。

本发明的一种基于视觉语义信息的地图构建方法，利用语义分割方法，可以直接从RGB图像中提取到每一个像素的从属语义，将图像按照语义信息分为不同的部分，再结合摄像机的内参，深度图像等其他相关信息，将语义像素投影到三维空间，形成点云。利用聚类方法，将三维空间中的语义点云聚类成为语义超点云。利用语义超点云作为地图的路标，构建起如图5所示的语义地图。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于视觉语义信息的地图构建方法，其特征在于，包括以下步骤：

S1、语义信息提取：利用神经网络提取图像的语义信息；

S2、语义超点云拟合：结合深度图像和摄像机内的参数，将语义像素投影到三维空间，并且进行聚类处理，得到语义超点云，所述语义超点云属于局部坐标系系统；

S3、地图增量式构建：根据摄像机外参数，将第一帧作为全局坐标系，将所有局部坐标系转换到全局坐标系，并对重叠的超点云进行加权合并；

S4、多地图融合：对于若干个局部地图，如果能找到相对位置关系的，根据相对位置关系进行步骤S3；找不到相对位置关系的，选择其中的一个作为全局坐标系，利用超点云计算其他地图到全局地图的匹配关系以得到相对位置关系，再进行步骤S3。

2.根据权利要求1所述的基于视觉语义信息的地图构建方法，其特征在于，所述步骤S1具体包括：

对于一帧RGB图像用F_i表示，其中包含nm个像素点p₁,p₂...p_nm，p_j＝(r_j,g_j,b_j)，分别表示该像素点的R、G、B分量，nm分别为该图像的高度和宽度，则神经网络处理过程表示为：

F′_i＝N(F_i) (1)

其中N表示语义分割神经网络处理过程，其接收一个图像作为输入，返回一个图像作为输出；F′_i表示经由神经网络处理后的图像，其中包含nm个像素点p′₁，p'₂...p'_nm，p'_j＝(l_j)，l_j表示该点的类别，l_j∈(L₁,L₂...L_n)表示l_j可能的取值，这是由神经网络的属性决定的。

3.根据权利要求2所述的基于视觉语义信息的地图构建方法，其特征在于，步骤S2所述的投影具体包括：

令在图像中的任一像素点O，该点投影到摄像机坐标系内的点P，则根据小孔透视成像模型，得：

其中(u,v,1)^T是O点在图像内的像素位置，(X,Y,Z)^T是P点在三维空间中的位置，K是相机内参，其中c_x、c_y分别表示光心在像素平面上的位置，f_x、f_y分别表示单位长度能表达的像素数；

对所有经由所述神经网络处理的图像进行投影，点云的数量为nm，取其中的一个点p_i＝(x_i,y_i,z_i,l_i)代表p_i在摄像机坐标系中X、Y、Z轴的分量以及所属的类别，l_j∈(L₁,L₂...L_n)代表所有可能的类型。

4.根据权利要求3所述的基于视觉语义信息的地图构建方法，其特征在于，所述聚类处理是根据点p_i的类别l_j的属性，选择以下步骤中的一个进行：

(1)对于一些适合用墙体进行拟合的物体，使用平面对所有符合的点云进行拟合，考虑属于类型l_k的点云p₁,p₂...p_n，使用平面对这些点云进行最小二乘拟合，即找到一个平面F＝(a,b,c,d)：

ax+by+cz+d＝0 (3)

其中a,b,c,d分别为定义该平面的参数，x,y,z分别为该平面的变量，满足待求解的最小二乘函数为：

在找到该平面后，规定a、b、c、d的符号必须满足以下特性：

ax₀+by₀+cz₀+d＞0 (5)

其中x₀,y₀,z₀分别代表摄像机的光心位置，即摄像机坐标系原点必须处于所有平面的法向量正方向，这个特性将在步骤S4中进行超点云匹配；

(2)对于一些适合用球体进行拟合的物体，使用椭球对点云进行拟合，考虑属于类型l_k的点云p₁,p₂...p_n，使用椭球对其进行拟合，假设一个椭球用s＝(x₀,y₀,z₀,A,B,C)表示，即：

其中x₀,y₀,z₀表示椭球的球心，A,B,C分别是在x,y,z轴分量的参数，满足待求解的最小二乘函数为：

5.根据权利要求4所述的基于视觉语义信息的地图构建方法，其特征在于，所述步骤S3具体包括：

将超点云转换到统一坐标系，定义误差函数F(x₁,x₂)计算两个超点云之间的相似度：

其中x₁、x₂分别代表两个用于比较的超点云，l₁、l₂分别是、x₂的类别，仅在两个超点云的类别相同时计算相似度，D代表两个超点云在同一个坐标系下的欧氏距离，G(x₁,x₂)为两个椭球的相似度，k₁...k₅分别是不同的系数，a₁、b₁、c₁与a₂、b₂、c₂分别是两个超点云所属椭球的参数；为了加快匹配速度，利用kd-tree组织超点云以加快查找速度；

如果一个超点云找到一个匹配，即该超点云与所有其他超点云计算相似度得到的最大值大于阈值k_corr，那么将这个超点云进行合并，如果一个超点云找不到一个匹配，即该超点云与所有其他超点云计算相似度得到的最大值小于阈值k_corr，那么将该超点云直接在全局空间坐标系中添加。

6.根据权利要求5所述的基于视觉语义信息的地图构建方法，其特征在于，所述步骤S4具体包括：

假定有若干个地图M1,M2...Mn，固定M1坐标系为全局坐标系，将其他地图转换到该坐标系，然后进行超点云合并；多地图融合分为两种情况：

a)各地图之间有相对位置关系的情况：对于有各地图之间的相对位置关系的情况，则直接对除了地图M1以外的地图Mn中的每一帧执行步骤S3，即地图增量式构建，在地图M1的坐标系即全局坐标系中插入、合并所有地图中的超点云；

b)对于各地图之间没有相对位置关系的情况，需要首先利用超点云建立匹配关系。

7.根据权利要求6所述的基于视觉语义信息的地图构建方法，其特征在于，建立超点云之间的匹配关系如下：

假设用于查询匹配关系的帧F'来自局部地图坐标系A，目的地图坐标系B包含帧F₁,F₂...F_n,首先利用词向量(BoW,Bag of Words)相关度建立初步匹配关系，假设所述神经网络提供的语义信息共有n类，即l_i∈(L₁,L₂...L_n)，则建立有关帧F的词向量v，v是一个n维向量，在第i维上的值表示在该帧F的周围k米内存在l＝L_i的超点云的个数，对于两帧F₁、F₂,根据两帧F₁、F₂的词向量计算相似度：

根据词向量相似度比较，在目的地图坐标系中找到帧F_k，F_k与待查询匹配关系帧F'相似度最高，因此，在F_k的周围的超点云中查找匹配关系；

在利用词向量相似度进行初步过滤之后，使用随机采样一致性(RANSAC)框架得到点云之间的匹配。

8.根据权利要求7所述的基于视觉语义信息的地图构建方法，其特征在于，使用随机采样一致性(RANSAC)框架得到点云之间的匹配关系，具体步骤如下：

1)对查询帧F中的相同类型的点云，随机赋予一个匹配关系；

2)在所有的匹配关系中，随机选择m个匹配关系并去除；

3)对剩下的匹配关系，利用基于SVD的ICP匹配求解方法计算转移矩阵T，并计算在T矩阵变换后，匹配的点云之间的欧式距离误差；

4)如果新的匹配关系带来的误差更小，那么记录这个匹配关系；

5)重复n次后，以匹配误差最小的T矩阵为最终的匹配关系，并利用平面语义来排除错误的匹配关系。

9.根据权利要求8所述的基于视觉语义信息的地图构建方法，其特征在于，步骤5)利用平面语义来排除错误的匹配关系具体如下：

假设拍摄帧F的摄像机坐标系原点在T矩阵变换下变换到世界坐标系p_world＝(x,y,z,1)，验证的平面为f＝(a,b,c,d)，如果：

ax+by+cz+d＜0 (10)

则表示该点云位于平面的后方，则需要抛弃该结果，找到T矩阵之后，为了排除误差，在RANSAC框架下寻找局部地图坐标系A与目的地图坐标系B之间的相对位置关系。

10.根据权利要求9所述的基于视觉语义信息的地图构建方法，其特征在于，寻找局部地图坐标系A与目的地图坐标系B之间的相对位置关系包括以下步骤：

(S1)对地图A中的每一帧，计算得到一个到达地图B的转移矩阵T，记为T₁,T₂...T_n；

(S2)对T₁,T₂...T_n中，随机认为其中k个矩阵正确，并用最小二乘法计算得到一个临时转移矩阵T_temp作为此次迭代选择的转移矩阵；

(S3)根据T_temp将地图B中的超点云

转换到地图A的坐标系，记为

对每一个超点云计算误差大小并求和作为整体误差，超点云的误差定义为

与其在地图A中的匹配超点云

之间的欧氏距离；

(S4)如果来自步骤S2、步骤S3的新的选择T_temp带来的整体误差更小，记录这个选择；

(S4)迭代M次数后，以误差最小的选择作为最终结果；

最终，得到地图A与地图B之间的相对位置关系，即T，根据该相对位置关系，执行步骤S3实现两个地图的合并。

Images