CN110097553B - 基于即时定位建图与三维语义分割的语义建图系统 - Google Patents

Abstract

本发明公开了一种新型的基于即时定位与建图和三维点云语义分割的语义建图系统，属于计算机视觉和人工智能技术领域。本发明利用即时定位与建图建立稀疏地图并获取关键帧和相机位姿，利用点云语义分割基于关键帧进行语义分割。利用二维目标检测方法和点云拼接获取平截头体建议，设计一种贝叶斯更新方案将候选平截头体的语义标签进行整合，将带有最终修正标签的点插入到建立的稀疏地图中。实验表明本系统具有较高的效率和准确率。

Description

基于即时定位建图与三维语义分割的语义建图系统

技术领域

本发明涉及计算机视觉技术领域，特别是涉及基于即时定位建图与三维语义分割的语义建图系统。

背景技术

服务型机器人一般由三个模块构成：人机交互、环境感知、运动控制。机器人感知周围环境，需要一套稳定、性能强大的传感器系统来充当“眼睛”，同时需要相应的算法和强有力的处理单元读懂物体。其中视觉传感器是不可或缺的一部分，相较于激光雷达、毫米波雷达，摄像头的分辨率更高，能获取足够的环境细节，如描述物体的外观和形状、读取标识等。尽管全球定位系统(Global Positioning System,GPS)有助于定位过程，但是由于高大树木、建筑、隧道等造成的干扰会使得GPS定位不可靠。

即时定位与建图(Simultaneous Localization and Mapping,SLAM)是指载有特定传感器的主体在没有先验信息的情况下，通过运动估计自身的轨迹，并建立周围环境的地图。其广泛应用于机器人，无人机，自动驾驶，增强现实，虚拟现实等应用中。SLAM可以划分为激光SLAM和视觉SLAM两类。由于起步早，激光SLAM在理论技术和工程应用上都较为成熟。然而高昂的成本、庞大的体积以及缺少语义信息使其在一些特定的应用场景中受限。根据相机类型，可将视觉SLAM分为三种：单目、双目以及深度SLAM。类似于激光雷达，深度相机可以通过采集点云来直接计算到障碍物的距离。深度相机结构简单，易于安装操作，而且成本低、使用场景广泛。

大部分的SLAM方案都是特征点或像素级别，为了完成一个特定的任务，或者与周围环境进行有意义的互动，机器人需要获取语义信息。SLAM系统应该能够选择有用信息，剔除无效信息。随着深度学习的发展，许多成熟的目标检测和语义分割的方法为精确的语义建图提供了条件。语义地图有利于提高机器人的自主性和鲁棒性，完成更复杂的任务，从路径规划转化为任务规划。

三维图像中的目标感知技术越来越成熟，三维理解的需求也越来越紧迫。由于点云的不规则性，大多数研究者会将点转化为规则的体素或者图像集合，利用深度网络进行预测。这样的转换不可避免地会导致数据量增大，空间点之间的相互关系被削弱。2017年提出的PointNet是第一个可以直接处理原始三维点云的深度神经网络。

现有大部分语义建图系统采用的是稠密建图，对二维图像进行语义感知，并将二维信息映射到三维环境模型中。首先稠密建图会导致系统性能下降，连续帧间存在信息冗余，有效利用帧间信息可以提高系统效率。对二维图像进行语义感知，无法充分利用空间信息，对于小物体和遮挡等情况较敏感。将二维信息映射到三维模型中，并利用条件随机场等方法对结果做修正。相比于直接对三维点云进行语义感知，这样的做法一定程度上降低了效率、削弱了系统性能。

发明内容

针对现有技术中存在的问题与不足，本发明提供基于即时定位建图与三维语义分割的语义建图系统，使用基于特征点的稀疏建图系统提取关键帧和相机位姿。针对关键帧，先利用成熟的二维目标检测方法提取感兴趣区域，再利用帧间信息即相机位姿，以及空间信息即图像深度，获取候选平截头体。利用点云语义分割方法对平截头体进行分割，设计一种贝叶斯更新方案将不同帧的分割结果进行融合。本发明旨在充分利用帧间信息和空间信息以提高系统性能，为达此目的，本发明提供基于即时定位建图与三维语义分割的语义建图系统，其计算处理步骤如下，利用稀疏建图技术与点云语义分割，建立包含感兴趣目标的半稠密语义地图，方法包括如下步骤：

(1)先对输入的图像帧序列进行即时定位与地图构建，获取关键帧与相机位姿。利用成熟的二维目标检测技术以及点云拼接，获得候选平截头体；

(2)再利用三维点云语义分割方法，对候选平截头体进行分割，利用贝叶斯更新方案，将多个候选平截头体的分割结果进行融合，将带有语义标签的空间点插入到稀疏地图中得到最终的半稠密语义地图。

作为本发明进一步改进，所述步骤(1)中先对输入视频帧序列进行即时定位与建图，再获取平截头体建议，具体步骤为：

步骤2.1：假设在时刻k，相机的位置为x_k，相机输入数据为u_k，x_k＝f(x_k-1，u_k，w_k)，w_k为噪声，将其称为运动方程，在x_k位置上观测到路标点y_j，产生一系列观测数据z_k，j，可以用观测方程来描述：z_k，j＝h(y_j，x_k，v_k，j)，其中v_k，j为观测噪声，这两个方程描述了最基本的SLAM问题：已知运动测量的读数u，以及传感器读数z，估计定位问题x和建图问题y；

在提取匹配特征点后，可以直接使用PnP估计相机的运动，采用EPnP计算初始位姿，核心思想是用4个控制点来表示空间点，然后构建重投影误差问题：

将估计的位姿作为初始值，采用相应方法可以求解，最后为解决累计漂移的问题，构造全局光束平差代价函数：

进行全局优化以解决累计漂移的问题；

步骤2.2：在获取到相机位姿和关键帧之后，利用二维目标检测方法SSD产生包围框、标签和置信度，本系统只考虑感兴趣目标，提取包围框内的RGB数据和深度信息。若包围框参数为(x，y，x₁，y₁)，包围框内像素的横坐标范围为[floor(rows*x)，ceil(rows*x₁)]，纵坐标范围为[floor(cols*y)，ceil(cols*y₁)]，其中floor为向下取整函数，ceil为向上取整，(rows，cols)为图像的尺寸，对于第i个像素I_refi＝[r_refi，c_refi]^T，深度为d_refi，相机坐标为：

步骤2.3：SLAM优化得到的位姿是旋转四元数的形式，一个四元数拥有一个实部和三个虚部：q＝q₀+q₁i+q₂j+q₃k，其中i，j，k满足：

对应的旋转矩阵为：

变换矩阵

左上角为旋转矩阵，右侧为平移向量，左下角为零向量，右下角为1。根据关系P_w＝T_wcP_c，可以得到平截头体内空间点的世界坐标。

作为本发明进一步改进，所述步骤(2)中获取候选平截头体，并对候选平截头体进行语义分割，具体步骤为：

步骤3.1：挑选具有固定间隔的关键帧以产生平截头体建议，给定间隔σ和序列长度S，第k个平截头体来自于第

个关键帧，k∈{1，...，σ}，剔除深度大于d_th的外点，先进行均匀采样，再将点云拼接得到最终的候选平截头体；

步骤3.2：直接对三维点云进行语义分割，基于点云语义分割网络，其对于输入遮挡和小物体具有较强的鲁棒性，首先对候选平截头体进行预处理，先进行随机采样至2048个点，转化为h5文件，再送入点云语义分割网络中，网络的的关键在于使用最大池化作为对称函数，对于点P_wi＝[x_wi，y_wi，z_wi]^T，i∈{1，...，N}，目标分类网络输出L个候选类别上的L个置信度，语义分割网络对于N个点和M个语义种类上的N×M个置信度。

作为本发明进一步改进，所述步骤(2)中利用贝叶斯更新方案对不同分割结果进行融合，设计方法为：

对于参考候选平截头体P_refw，

表示其在第m个候选平截头体中的前向投影点，只有当

时，这个前向投影点才可以作为最终标签的影响因素，ε₁的值随着场景的变化会有所不同；

给定一个点P_refwi和其对应的前向投影点集

用z_refi表示分配给点P_refwi的语义标签，标签集合为

对于标签l，可以得到：

本发明基于即时定位建图与三维语义分割的语义建图系统，有益效果如下；

本发明提供的基于即时定位建图与三维语义分割的语义建图方案可以高效准确地将输入视频帧序列转化为半稠密语义地图。本发明建立稀疏地图，并且只对关键帧进行处理，提高了语义建图系统的效率。充分利用空间信息和帧间信息，提高了系统性能，尤其对于遮挡和小物体等有较强的鲁棒性。本发明适用于很多场景，稀疏定位建图系统可以建立室内或室外场景的地图，基于深度学习的二维目标检测技术和三维点云语义分割方法只需要有效的训练便可以检测海量物体。

附图说明

图1为本发明基于SLAM和三维语义分割的语义建图系统流程图；

图2为本发明利用SLAM获得关键帧和相机位姿的流程图；

图3为本发明Point-Net的网络架构图；

图4为本发明利用ORB-SLAM获取的稀疏地图，包含相机位姿以及关键帧；

图5为本发明关键的中间结果图，依次为：(1)二维目标检测产生目标候选区域和类别置信度；(2)提取平截头体中的点云；(3)挑选固定间隔的关键帧产生的平截头体点云，进行均匀采样并拼接；(4)最终产生的包含感兴趣目标的语义地图；

图6为本发明最终获取的半稠密语义地图示例。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供基于即时定位建图与三维语义分割的语义建图系统，使用基于特征点的稀疏建图系统提取关键帧和相机位姿。针对关键帧，先利用成熟的二维目标检测方法提取感兴趣区域，再利用帧间信息即相机位姿，以及空间信息即图像深度，获取候选平截头体。利用点云语义分割方法对平截头体进行分割，设计一种贝叶斯更新方案将不同帧的分割结果进行融合。本发明旨在充分利用帧间信息和空间信息以提高系统性能。

下面基于Ubuntu16.04和Nvidia Geforce GTX 1080，借助于Tensorfiow ObjectDetection API、OpenCV、Point Cloud Library等工具，在TUM数据集rgbd_dataset_freiburg1_plant上为例，结合附图对本发明基于即时定位建图与三维语义分割的语义建图方案的具体实施方式作进一步详细说明。

步骤1：基于即时定位与建图获取稀疏地图、关键帧以及相机位姿，如图4所示。在特征点提取匹配阶段，FAST检测过程为：

1.在图像中选取像素p，亮度为I_p，设置阈值T；

2.以像素p为中心，选择半径为3的圆上的16个像素点；

3.如果圆上有连续的N个点的亮度大于I_p+T或小于I_p-T，那么像素p可被认为是特征点。

灰度质心法的步骤包括：

1.定义图像块B的矩为：m_pq＝∑_x，y∈Bx^py^qI(x，y)，p，q∈{0，1}；

2.图像块的质心为：

3.连接图像块的几何中心O和质心C，得到方向向量

特征点的方向可以定义为：θ＝arctan(m₀₁/m₁₀)。

在局部BA中，重投影误差关于相机位姿李代数的一阶变化关系推导为：

重投影误差关于空间点的倒数推导为：

步骤2：基于关键帧和二维目标检测获得平截头体建议。首先利用SSD检测出目标的候选区域，得到包围框的参数(x，y，x₁，y₁)，因此包围框内像素的横坐标范围为[floor(rows*x)，ceil(rows*x₁)]，纵坐标范围为[floor(cols*y)，ceil(cols*y₁)]，其中floor为向下取整函数，ceil为向上取整，(rows，cols)为图像的尺寸。假设上一步SLAM估计得到的该关键帧的位姿信息为(t₁，t₂，t₃，q₁，q₂，q₃，q₀)，构造旋转矩阵：

平移向量为t＝[t₁，t₂，t₃]^T，因此转换矩阵为

对上述范围内的像素进行遍历，相机坐标p_c到像素坐标(u，v，d)的关系为：

反过来，设p_c＝[x，y，z]，那么：

根据关系P_w＝T_wcP_c可以获得包围框内像素对应的空间点的世界坐标，利用PCL库构造平截头体内的点云，至此获得每个关键帧的平截头体建议。

步骤3：获取候选平截头体并进行语义分割。挑选具有固定间隔的关键帧产生的平截头体建议，给定间隔σ＝15和序列长度S＝195，第k个平截头体来自于第{15n+k，n∈{0，1，...12}}个关键帧，k∈{1，...，15}，剔除深度大于0.7米的外点。先对每个候选平截头体进行均匀采样，然后进行点云拼接。对得到的拼接点云进行随机采样至2048个，将平截头体旋转到中心视角，使其中心轴与图像平面正交。在送入Point-Net之前，将点云的XYZ坐标值减去中心点的值获得局部坐标。将二维检测器检测出的语义类别编码为一个独热类别向量，并将其拼接到中间点云特征中。

步骤4：基于贝叶斯方案将多帧结果进行融合。对于参考候选平截头体P_refw中的点P_refwi，

表示其在第m个候选平截头体中的前向投影点。做一个几何校验，将

转换到参考坐标系，只有当

时，这个前向投影点才可以作为最终标签的影响因素，ε₁为5毫米。

给定平截头体中的一个点P_refwi和其对应的前向投影点集

用z_refi表示分配给点P_refwi的语义标签，标签集合为

即分配给该点的标签以及分配给其前向投影点的标签。对于标签l：

其中

表示分配给前向投影点但未分配给参考点的标签。在独立同分布假设下，假设所有类别的先验概率相同，上式可以简化为：

步骤5：将最终带有语义标签的点云插入到之前建立的稀疏地图中，以建立带有感兴趣目标的半稠密语义地图，如图6所示。在总的195帧关键帧中，针对植物这一类别，SSD在63帧中存在漏检情况。在获得点云分割结果之后，将带有语义标签的点反向投影到二维图像中，平均准确度为90.2％。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (1)

1.基于即时定位建图与三维语义分割的语义建图系统，其计算处理步骤如下，其特征在于：利用稀疏建图技术与点云语义分割，建立包含感兴趣目标的半稠密语义地图，方法包括如下步骤：

(1)先对输入的图像帧序列进行即时定位与地图构建，获取关键帧与相机位姿，利用成熟的二维目标检测技术以及点云拼接，获得候选平截头体；

所述步骤(1)中先对输入视频帧序列进行即时定位与建图，再获取平截头体建议，具体步骤为：

步骤2.1：假设在时刻k，相机的位置为x_k，相机输入数据为u_k，x_k＝f(x_k-1，u_k，w_k)，w_k为噪声，将其称为运动方程，在x_k位置上观测到路标点y_j，产生一系列观测数据z_k,j，可以用观测方程来描述：z_k,j＝h(y_j，x_k，v_k，j)，其中v_k，j为观测噪声，这两个方程描述了最基本的SLAM问题：已知运动测量的读数u，以及传感器读数z，估计定位问题x和建图问题y；

在提取匹配特征点后，可以直接使用PnP估许相机的运动，采用EPnP许算初始位姿，核心思想是用4个控制点来表示空间点，然后构建重投影误差问题：

将估许的位姿作为初始值，采用相应方法可以求解，最后为解决累许漂移的问题，构造全局光束平差代价函数：

进行全局优化以解决累许漂移的问题；

步骤2.2：在获取到相机位姿和关键帧之后，利用二维目标检测方法SSD产生包围框、标签和置信度，本系统只考虑感兴趣目标，提取包围框内的RGB数据和深度信息，若包围框参数为(x，y，x₁,y₁)，包围框内像素的横坐标范围为[floor(rows*x)，ceil(rows*x₁)]，纵坐标范围为[floor(cols*y)，ceil(cols*y₁)]，其中floor为向下取整函数，ceil为向上取整，(rows，cols)为图像的尺寸，对于第i个像素

深度为d_refi，相机坐标为：

步骤2.3：SLAM优化得到的位姿是旋转四元数的形式，一个四元数拥有一个实

和三个虚

：q＝q₀+q₁i+q₂j+q₃k，其中i，j，k满足：

对应的旋转矩阵为：

变换矩阵

左上角为旋转矩阵，右侧为平移向量，左下角为零向量，右下角为1，根据关系P_w＝T_wcP_c，可以得到平截头体内空间点的世界坐标：

(2)再利用三维点云语义分割方法，对候选平截头体进行分割，利用贝叶斯更新方案，将多个候选平截头体的分割结果进行融合，将带有语义标签的空间点插入到稀疏地图中得到最终的半稠密语义地图；

所述步骤(2)中获取候选平截头体，并对候选平截头体进行语义分割，具体步骤为：

步骤3.1：挑选具有固定间隔的关键帧以产生平截头体建议，给定间隔σ和序列长度S，第k个平截头体来自于第

个关键帧，k∈{1，...，σ}，剔除深度大干d_th的外点，先进行均匀采样，再将点云拼接得到最终的候选平截头体；

步骤3.2：直接对三维点云进行语义分割，基于点云语义分割网络，其对于输入遮挡和小物体具有较强的鲁棒性，首先对候选平截头体进行预处理，先进行随机采样至2048个点，转化为h5文件，再送入点云语义分割网络中，网络的的关键在于使用最大池化作为对称函数，对于点P_wi＝[x_wi，y_wi，z_wi]^T，i∈{1，...，N}，目标分类网络输出L个候选类别上的L个置信度，语义分割网络对于N个点和M个语义种类上的N×M个置信度；

所述步骤(2)中利用贝叶斯更新方案对不同分割结果进行融合，设许方法为：

对于参考候选平截头体P_refw，

表示其在第m个候选平截头体中的前向投影点，只有当

时，这个前向投影点才可以作为最终标签的影响因素，ε₁的值随着场景的变化会有所不同；

给定一个点P_refwi和其对应的前向投影点集

用z_refi表示分配给点P_refwi的语义标签，标签集合为

对于标签l，可以得到：

Images