语义信息增强的激光雷达SLAM

作者简介

本次深蓝学院公开课由陈谢沅澧博士主讲。陈谢沅澧来自德国波恩大学图像测量与机器人实验室，导师是Cyrill Stachniss教授，主要方向是激光雷达SLAM，定位以及建图。

德国波恩大学图像测量与机器人实验室长期招收博士生，如果大家感兴趣，可以在官网上（www.ipb.uni-bonn.de）查找相关信息。

打开百度APP，查看更多高清图片

实验室简介：主要有两个大的课题。

一是农业机器人，包括农作物的识别与分类，土壤成分的分析与检测，农业机器人设计以及相关的机器人技术（定位、建图、导航等）。

二是移动机器人与无人车。下图为作者实验室的移动平台，搭载了两个激光雷达，分别是64线和128线的，同时搭载了4-8个摄像头，以及一些IMU和GPS传感器。

下图为实验室与业界的一些合作。包括Google和Facebook。

下图为实验室19年发表的semantic KITTI，对KITTI数据集中的激光雷达数据添加语义标签，用来测试语义分割、语义SLAM等相关算法。

下图为实验室和意大利教廷合作的项目，地下遗址探索机器人。

本次讲述内容：

1. SuMa++: Efficient LIDAR-based Semantic SLAM, IROS2019

Code: https://github.com/PRBonn/semantic_suma/

2. OverlapNet: Loop Closing for LIDAR-based SLAM, RSS2020

Code: https://github.com/PRBonn/OverlapNet

3. Learning an Overlap-based Observation Model for 3D LIDAR Localization, IROS2020

Code: https://github.com/PRBonn/overlap_localization

1.SuMa++: Efficient LIDAR-based Semantic SLAM

主要目标：同时使用几何和语义信息的激光雷达SLAM系统，生成一个全局一致的稠密的语义地图。如图所示。

动机

下图为纯几何激光雷达SLAM的问题。左边展示的是结果，红色的车是位姿的真值，蓝色的车是SLAM系统估计出来的位姿。可以看出右边的大卡车遮盖了很多的视野，并且卡车正在向前移动，以本车的视角出发，本车相对卡车是向后退的，由此构建的系统估计的位姿是错误的向后退的，这在实际过程中是很致命的错误。

下图为使用了语义信息，与之前不同的是，此时我们去除掉大部分属于移动车辆，这样移动的卡车对系统的影响就很小了。结果可以看出估计的位姿与真值十分接近(红蓝两车基本重合)。这是我们想要的结果，利用语义信息来提高3D激光雷达SLAM的性能。

系统框架

1. 系统的输入

2. 语义分割

3. 预处理

4. 建图

5. 里程计

（1）系统输入

仅使用激光雷达点云进行建图，与传统激光雷达SLAM不同的是，作者不是直接对原始点云进行处理，而是将原始点云投影成距离图像(range image)。如图所示，它类似于一个360度的全景图像，点云中的值被投射到2D图像之中，图中的像素包含了激光雷达点云测量的距离等信息。

为什么要使用range image呢？

1. 比较直观的是，range image可以把稀疏的点云表达成稠密的图像，基于图像可以使用许多经典的图像处理方法来处理点云，并且实现类似于图像处理的功能，例如目标识别、语义分割等。

2. 降维。可以把三维激光雷达点云投影成二维图形，可以提高点云处理的速度和占有的空间。

3. 可以使用GPU对SLAM系统进一步加速，更加容易使用深度学习算法进行处理。

（2）语义分割

本文使用rangenet++这篇论文的方法，利用的就是range image，算法的核心思想是将激光雷达点云投影成二维图像，基于图像的深度学习算法对图像进行语义分割，再重投影到三维点云中，从而对每个测量点进行语义标注，左上角是原始点云，左下角是range image，右下角是range image的分割结果，将分割结果和原始点云结合起来，生成右上角的基于点云的分割结果，作为输出。

（3）预处理

对激光雷达生成的信息进行预处理，包含深度信息，法向量信息，语义信息等。在预处理过程中，在激光雷达生成的不同类型的信息进行初步融合，即多类洪水填充算法。

核心思想是利用深度信息对语义分割中的噪点进行去除。实际操作也很简单，在生成激光雷达语义图像之后，首先遍历语义图像中所有像素，检测相邻像素的语义标签是否一样。如果不一样，首先对他们进行腐蚀(图像处理中的腐蚀运算)，再检测所对应的像素的深度值，若深度值差值很小，则认为他们在同一个物体上，用相邻像素的语义标签来填充被腐蚀掉的像素。整个过程就是对range image的基于语义信息的平滑操作，以达到去噪的效果。

（4）建图

此模块是基于语义信息对动态物体进行滤除。

文章提出的算法是基于SuMa(同样出自作者的实验室的论文)中的算法进行改进的。所以在建图的环节沿用了之前面元地图的表达形式。首先介绍一下面元(surfel,具体可参考SuMa文章定义)地图的概念。

面元地图：点云地图的一中扩展。将点云信息中法向量信息一致的相邻点用同样一个面元进行表示。既减少了存储点的数量，又提供了更多了的几何信息。例如面元相对于点云而言，不光是xyz的位置信息，还包含了法向量信息、面积信息以及稳定性信息和遮挡关系的信息。除了几何信息外，还加入了语义标签和对应的概率信息，生成了带语义信息的地图，如左图所示。

在得到了带有语义信息的面元地图后，接下来的任务就是如何利用语义信息去除动态物体。最直接的做法就是删除所有潜在的移动物体，叫做Remove Movable。如右图所示，删除所有可能移动的物体后。

但是这种做法是最优的吗？

这种做法对于长期定位而言可能是由帮助的，因为在全局定位中是时间比较长的任务，车辆和行人大概率是会移动的，所以删除它们会对长期定位起到帮助作用。但对于SLAM-即时定位与建图而言，停靠在路边的静止的车辆，对于SLAM建图会有帮助作用。

这时候就需要设计一种可以去除正在移动的物体，但仍保留一些可能运动的物体的算法，如中间图所示。算法的实现是利用了面元的一致性检测。即在更新面元稳定性（ls）时，除了检测几何空间一致性，还检测语义信息一致性。

几何空间(geometric)一致性：同一物体在不同观测中的空间信息是一致的。包括位置信息、法向量朝向信息。

语义信息(semantic)一致性：同一静止物体在不同观测中的语义标签是一致的。对于移动物体而言，同一位置对应的语义标签会发生改变。

方法：当检测到语义标签发生改变时，对面元的稳定性在更新时，给定一个额外的惩罚。在多次观测后，属于移动物体的面元的稳定性会很低，当低于某一阈值时，把这个面元滤除掉。而对于静止的物体而言，稳定性会较高，则会被保留下来。

（5）里程计

使用帧到模型的ICP(frame-to-model ICP)，通过最小化当前帧和模型帧之间的残差来估计激光雷达的位姿。

模型帧：渲染已经建好的地图得到一个range image。好处是通过帧到模型的匹配的方式，相较于传统的ICP算法，不需要搜索最近邻，因为可以利用投影关系来找到匹配点，很大的提高运算效率。类似于基于图像的光流法，但比光流法更好的是它使用了地图模型的法向量信息和其他的几何信息。

在ICP过程中，利用语义信息来提高位姿估计的精度。具体做法是计算ICP的信息矩阵时，对每个残差进行加权。在传统的ICP中会使用一些基于几何信息的加权，例如Huber加权。本文还使用了语义信息进行加权，检测当前帧和模型帧的语义标签的一致性，如果不一致，使用很小的权值去降低噪点或权值，当标签一致时，直接利用语义分割估计的概率进行加权。

文章对ICP过程中的权值进行了可视化。如图所示。颜色越暗，权重越小。

测试结果：移动的车受到抑制，静止的车得到保留，融合了语义信息之后，位姿估计更准确。

总结：

1.提出了一个高效的基于激光雷达的语义SLAM系统。

2.对如何在激光雷达SLAM中使用语义信息提供了三个事例，包括基于深度信息的多类洪水填充算法，对输入进行预处理；基于语义的动态物体检测与去除；基于语义信息的ICP。

3.实验结果证明，在使用语义信息之后，文章的SLAM系统可以得到更好的定位和建图结果。相较于简单粗暴的删除所有可能移动的物体，文章的方法可以得到更鲁棒的效果。

2.OverlapNet: Loop Closing for LIDAR-based SLAM

动机：对于经典的SLAM系统中，包含了三个大部分：定位、建图、闭环。其中闭环检测是很重要的。在长期系统运动中，由于系统误差和测量噪声，SLAM系统往往会产生一定的漂移，闭环就是用来消除漂移，提高SLAM整体定位和建图的精度。

概要：提出了OverlapNet神经网络，利用端到端的深度学习方式，来估计两帧激光雷达点云之间重叠部分（overlap）大小和相对偏航角，基于两个估计来解决SLAM闭环问题。

什么是闭环？

闭环是当机器人或无人车在进行SLAM过程中，回到曾经到过的地方，能否检测出当前位置是曾经到达的位置，能否估计出当前位姿和历史图像之间的相对变换，从而给后端优化提供额外的约束，消除掉累计产生的漂移。

什么是overlap？

文章将两个激光雷达点云之间的overlap定义为同时能够从两个激光雷达点云中可以观测到的点的频率。即如果一个激光雷达的测量点能够同时被两个激光雷达点云所包含，就将其记为一个重叠点。两帧激光雷达点云中重叠部分的大小定义为重叠点在所有观测点中所占的比例。类似于目标检测的IoU的概念，即表示两次得到的点云数据的重合度。

为什么要用overlap来寻找闭环呢？

因为观测到相近位置的激光雷达点云的overlap往往比较大，比较远的激光雷达点云的overlap比较小。所以overlap很好的包含了两帧激光雷达点云中的距离信息。除此之外，更大的overlap同时意味着两帧激光雷达可以更好的进行对齐。如下图所示。

上图中蓝色的点云A，橙色的点云B，如果将两者放到错误的位置上，那么之间的重叠率会很低，如果放在正确的位置，那么他们的重叠率会很高。这就意味着两个激光雷达点云的overlap同时包含了他们的相对位置信息。

图为文章提出的框架。

左边为预处理过程，和SuMa++类似，将输入的激光雷达点云预处理生成不同的信息的range image，包含深度信息，法向量信息，强度信息以及语义信息，这样就可以利用基于图像的2D卷积神经网络来实现不同的功能。

右边为OverlapNet的框架，由两条腿和两个头组成。两条腿使用的神经网络具有相同的结构，是一个简单的CNN，之间共享权值。两个头同时使用由两条腿生成的特征，Delta Head估计两帧点云的overlap，Correlation Head估计两帧点云的偏航角(yaw)，这个网络结构很类似于单目标跟踪中的孪生网络，先过一个共享权值的网络，将特征映射到同一个表征空间，然后做特征融合。

这样的头脚分离框架的好处，一是对于SLAM而言，将每一帧生成的特征直接存储在地图中，因此在进行闭环检测的时候，仅需要对当前帧的特征和对所有历史帧的特征进行比较。特征之间的比较是十分高效的，很大程度的提高运行效率。二是它非常的灵活，可以根据自己应用的需求添加其他的头来实现不同的功能。

接下来展示文章所提出来的算法和其他算法的比较。

图中展示了文章算法的泛化能力，在两个不同的数据集上进行了测试。虽然OverlapNet仅仅使用了KITTI部分数据集进行训练，但在FordCampus中也表现出了比较好的性能。这表明了文章的算法在不同的环境中的较强的泛化能力。

下图展示了OverlapNet嵌入到激光雷达SLAM系统中的运行结果。左边为SLAM的局部地图，右边为相对应的位姿图，颜色越深，误差越大。使用OverlapNet进行的闭环检测，可以有效减少误差。

下图为与纯几何信息的SLAM进行比较的结果图。在加入了闭环检测的算法后，定位和建图的精度有很大的提高。

总结：

1. 提出了一个基于深度学习的激光雷达检测算法OverlapNet，可以准确的估计两帧之间重叠部分的大小以及偏航角。

2. 可以在极具挑战的情况下仍然能找到闭环，例如当车从反方向开回去的时候仍然可以找到闭环。

3. 通过实验数据，验证了OverlapNet的泛化能力很强，在不同的数据集中，表现出了很好的性能。

4. 成功地把OverlapNet集成到了SLAM系统中，提高了激光雷达SLAM的建图精度。

3.Learning an Overlap-based Observation Model for 3D LIDAR Localization

动机：利用地点识别算法实现全局定位的方式有很多，比如基于图像检索的拓扑定位，好处是直接用一个地点识别算法得到一个粗略的位置，但往往精度会比较差。文章使用了OverlapNet作为蒙特卡洛定位算法（MCL）的观测模型，实现高精度的传感器定位。

目前MCL最大的难题就是如何去设计一个好的观测模型。

文章的创新点是利用OverlapNet来训练了一个观测模型，然后把它集成到MCL中，提高了MCL的定位性能。

什么是MCL？

MCL包含了两个部分，跟所有的传统的贝叶斯模型一样，包含了预测部分(motion model)和更正部分(observation model)。不同的是，MCL利用了粒子滤波对运动模型进行采样，每个粒子可以当作对当前位姿的假设(hypothesis)。使用粒子滤波的好处是可以对整个空间进行采样，MCL可以在不知道初始位置的情况下，对多个假设进行跟踪，最后通过连续不断的观测和纠正，选择出正确的位姿估计，实现全局定位，使整个算法更加鲁棒。

运动模型：利用上一个位姿xt-1，以及当前里程计的信息ut，对每个粒子j在当前时刻的位姿xt进行预测。

观测模型：在给定当前粒子的位置xt和地图m，利用当前的观测zt来调整每个粒子j的权值，最后对整个位姿后验进行更新。

图为Overlap观测模型

此模型与上一篇介绍的模型类似，不同的是输入不再都是由激光雷达生成的range image，Query scan是当前激光雷达传感器的观测，Map scan是从地图中所渲染生成的虚拟的range image。生成方式与SuMa++中类似。

接下来介绍如何使用Overlap传感器模型更新粒子的权值。

得到关于当前帧和地图帧之间的Overlap和Yaw的预测值之后，利用他们对粒子的权值进行更新。相对于传统的传感器模型，我们进行分解，OverlapNet可以得到两个输出，其中Overlap当作对位置的似然估计，偏航角Yaw当做对旋转的似然估计。

然后对观测模型进行解耦。针对每一个粒子假设的位置和偏航，分别计算权重。

在计算位置的似然时，使用Overlap的估计值，因为它本身包含了两帧激光雷达点云之间的位置信息。在计算偏航的似然估计时，使用高斯模型进行估计，最后将两者结合起来，对粒子的权值进行更新。过程如下：

下图展示了实验过程中使用的全局地图，蓝线表示曾经去过的地方。

取其中一小块地图作为示例。

首先对整个环境进行栅格化，然后对每个栅格从环境模型中渲染出虚拟的range image，存储到地图中。图中取了3个粒子。根据每个粒子的位置，赋予它们相应虚拟的地图帧。

然后将当前帧(Query scan)和每个粒子的地图帧进行比较，即将它们作为OverlapNet的输入，得到输出后通过之前介绍的分解观测模型，对每个粒子的权值进行更新，最后将观测模型结合到MCL的定位中，从而实现全局定位。

下图为整个系统的运行结果。左上角是全局定位的结果，首先对整个地图进行初始化采样，在很短的时间内，所有的粒子群可以收敛到正确的位置。右上角是局部跟踪的结果。左下角是当前帧和所估计出来的位姿所对应的地图帧。

最后展示的定量比较的结果，显示了文章算法的泛化能力。地图是在春天进行采集的，测试定位所用的数据集是在秋天和冬天进行采集的。可以看出文章的算法优于传统的算法的性能。

总结：

1. 提出了基于激光雷达的全局定位系统，即新的基于深度学习的OverlapNet观测模型。

2. 提出的算法具有好的泛化能力。

3. 将提出的模型集成到MCL中，定位系统能够很好的实现全局高精度定位。

希望通过本次公开课，大家可以对语义信息增强的激光雷达SLAM现在在做的一些工作和研究有一定的了解！

感谢深蓝学院的张铮同学，也感谢大家对深蓝学院的支持