[游戏开发]语义激光SLAM论文阅读 Semantic Lidar_based SLAM paper Research

Suma: Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments

主要工作

1. 提出了基于激光雷达3D点云的SLAM系统，能够有效的建立全局一致的地图。
2. 提出了一种基于地图的新的回环检测方法，即使在两帧交叠很少的情况下也能检测到回环。

具体实现方法

原始点云经过预处理（投影）生成顶点图和法向量图，一部分用于和基于面元的地图匹配生成里程计信息，另一部分结合里程计信息生成面元地图，还有一部分用于回环检测，优化面元

Preprocessing

首先将3D点云投影成vertex map，实际上就是将圆柱形视场展开成一个矩形。

法向量图normal map计算方式：

通过投影计算法向量能大大减小计算的压力。
投影结果：

顶点图vertex map的颜色代表距离，法向量图normal map的颜色代表法向量的方向。

Map Representation

基于surfel的地图能表示大范围地图的同时，还能保持稠密度和细节的几何信息。每个surfel包含中心点的坐标、法向量、surfel的半径、创建的时间戳和最后一次更新的时间戳==（用于判断该surfel是否是active，active surfel用于雷达里程计，inactive surfel用于回环检测）==。

对于每一个面，都有一个指标来判断面是否稳定。

Odometry Estimation

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

frame-to-model误差函数：

Map Update

判断vertex map中的哪些点要融合到surfel map中的已存在的surfel中，或者是重新产生一个surfel。

通过投影关系可计算出data surfel s中的$v_s,n_s$，$r_s$通过以下公式计算：

现有最新观测得到的data surfel（$v_s,n_s,r_s$）和model surfel（$v_s^{,},n_s^{,},r_s^{,}$）

如果满足：

就认为data surfel和model surfel是兼容的，融合两者同时增加稳定性。否则，认为data surfel和model surfel不兼容并初始化一个新的surfel，地图更新之后，移除所有稳定值过低同时太旧的surfel。

Loop Closures

Detection

在不活跃的地图$M_{inactive}$里找，从开始位姿到当前时刻减掉delta active范围的位姿里找符合条件的j*。对于j*，用frame-to-Model ICP将对应位姿$T_{W{C}_{j?}}$??处的渲染视图与当前点云对齐。（设置了多个ICP初值，挑选效果最好的一个。）

为了解决扫描间重叠率低的情况，我们使用里程计估计和上述候选方法的转换，在潜在的回环之后绘制了地图的虚拟视图。对于重叠率低的情况，不能只简单地评估当前测量值相对于渲染的不活跃的地图$M_{inactive}$?的残差。

即使我们尝试通过渲染虚拟地图来排除无效的回环，该标准仍可能导致无效的回环，因此我们需要验证回环（通过追踪active map和inactive map中的位置）。

主要就是检测回环之后有估计了一个里程计的增量，并将该里程计的增量用于地图的active和inactive部分的位姿。接着用这些位姿渲染地图的虚拟地图，并用当前测量值再次检查一致性（Decetion中虚拟视图的部分）。

理解与思考

• SuMa本身不是一种基于语义的SLAM方法，但是为了介绍后续的SuMa++，在这里进行了介绍。
• SuMa与其他常用的SLAM方法（如LOAM，LeGo-LOAM等）不同的地方在于它基于面元建图和定位。

SuMa++: Efficient LiDAR-based Semantic SLAM——波恩大学

主要工作——2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

1. 本文将语义信息融合到基于surfel的地图上，并提出了一种用语义标签过滤动态物体的方法。

2. 之前有些语义SLAM的主要目标是在语义类非常有限的情况下，为全局检索和多机器人协作SLAM提取有意义的特征。与之相反，本文专注于生成具有丰富语义信息的语义地图，并使用这些语义过滤动态对象（如移动的车辆和人），以提高地图和里程计的准确性。

具体实现方法

使用的是SuMa的主干。3D点云经过投影之后获得一张长方形距离图像，使用RangeNet++网络来预测每个点的语义信息，并产生一个mask。在预处理阶段，使用了洪水填充算法给语义距离图像去噪，在地图更新的过程中。添加了一个动态物体检测和去除的模块。同时在前端匹配部分添加了语义的约束（使用语义信息给SuMa的ICP加权）。

Surfel-based Mapping

3D点云投影生成顶点图，顶点图计算生成法向量图，以上两张图和地图进行ICP的匹配更新位姿。并且使用这些位姿构建surfel-based map和回环检测。

Semantic Sagmentation

输入的是经过球面投影时候的图像，使用的是RangeNet++网络，该网络基于SqueezeSeg架构，使用DarkNet53主干网络，数据集是SemanticKITTI，该网络可以给每个点都提供一个语义标签。本文给每个surfel附加了一个语义标签和该标签在分割时的概率。

从网络输出的投影图需要通过flood-fill算法处理误差。

算法流程：

1. 边界侵蚀：为了消除分割边界的模糊

   在输入的语义mask中，如果某个点的固定邻域中有其他的点的语义标签与该点不同，则删除该点，得到侵蚀后的语义mask。

2. 将侵蚀的语义mask，与从Vertex map 中得到的深度图结合，得到Fill-in mask。

3. 最后，对于语义标签空白的点云（应该是第一步被移除的点云），如果其领域内有其他点，则把该空白点的标签置为其邻域内点的标签（边界噪声过滤）。

效果：

Filtering Dynamics using Semantics

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

动态物体去除的效果：

Semantic ICP

在frame-to-model ICP部分，本文也计入了语义的约束来较少离群点的约束。误差函数为：

总结和思考

1. 提出了一个高效的基于激光雷达的语义SLAM系统。
2. 本文在激光SLAM中的三个模块使用了语义信息，包括基于深度信息的多类洪水填充算法，对输入进行预处理；基于语义的动态物体检测与去除；基于语义信息的ICP。
3. 实验结果证明，在使用语义信息之后，文章的SLAM系统可以得到更好的定位和建图结果。相较于简单粗暴得删除所有可能移动的物体，本文的方法可以得到更鲁棒的效果。

PSF-LO: Parameterized Semantic Features Based Lidar Odometry——阿里巴巴

主要工作——International Conference on Robotics and Automation (ICRA) 2021

1. 本文设计了四种类型的轻量级点云函数：道路、建筑、交通标志和杆状，以抽象和精确地参数化它们。

2. 本文使用参数化语义特征和几何特征来互相弥补不足，以实现低漂移和实时的雷达里程计。

3. 本文设计了一种新的动态和静态目标分类器，同时考虑到目标的时间航向一致性、速度大小和不确定性，在配准过程中识别和去除动态目标，进一步消除雷达里程计漂移。

SuMa++使用语义信息在surfel级别过滤动态物体，并实现语义ICP。本文使用参数化语义特征优化几何特征匹配，并在实例级别对静态和动态障碍点进行分类。

具体实现方法

原始点云输入到语义分割网络（RangeNet++）中获得语义点云，用于雷达里程计模块、PSF子图匹配模块和动态物体剔除模块。雷达里程计首先移除语义点云中离群的几何特征，然后进行帧间匹配输出高频的位姿预测，语义点云同时进行子图匹配来限制累计误差。第二条线路从语义点云挑选出四种特征，对每一个特征实现参数化，最后参数化语义特征和参数化语义子图进行匹配，匹配的位姿用于细化LOAM框架中的子图匹配位姿。第三条线路用于剔除动态特征，剩下的静态特征用于LOAM框架中的子图匹配。最终的融合位姿用于参数化语义特征子图的更新和动态特征的筛选。

PSF Extration

本文自定义了四种参数化语义特征{road, building, sign, pole}。在每个PSF对应一系列参数化方程系数、权重系数、语义标签、边界框中心点、用于渲染的边界框轮廓。

1. Road PSF：为避免近密远疏带来的影响，本文使用多分辨率2D栅格将空间划分为一系列栅格。在每个栅格内都使用RANSAC平面拟合，得到平面方程系数，权重等于有效点占总点数的比例，最后计算所有有效点的最小封闭矩形，使用矩形中心点和四个角作为road PSF。

2. Building PSF：将栅格旋转45度，每个栅格里面进行多平面提取。

3. Traffic sign PSF：直接使用欧几里得聚类进行分割，然后使用多平面提取。

4. Pole-like PSF：先使用欧几里得聚类，然后拟合出一条3D直线，用一个杆上离原点距离最近的点坐标和方向向量表示。

Lidar Odometry Based on PSFs

使用PSF对LOAM进行优化：

1. 过滤掉几何特征中不可靠的边特征（在road PSF或者其他有平面的PSF上的点）
2. 添加语义一致性惩罚，如果在匹配的时候语义标签不一致，权重会减小。

使用PSF辅助子图匹配的结果：

Dynamic and Static Object Classification

实验结果

总结和思考

1. 本文主要是设计了四种参数化语义特征来细化结合特征匹配，并提出了一个动态和静态物体分类算法来提高雷达里程计的准确率。

SA-LOAM: Semantic-aided LiDAR SLAM with Loop Closure——浙大&华为

主要工作——ICRA 2021

1. 本文提出了一种基于激光雷达的语义SLAM系统，能够在大规模场景下创建连续的语义地图。
2. 本文提出了一种基于LOAM的语义ICP方法，能够提高雷达里程计的准确率。
3. 本文提出了一种基于语义的回环检测方法，并维护一个位姿图用于有效的回环检测和限制累计误差。
4. 在KITTI和牛津大学的数据集实验，与流行的方法比较，本文的方法在准确率方面有较大提升。

具体实现方法

原始点云输入到语义分割网络（RangeNet++）中获得语义点云，语义点云用于雷达里程计和语义位姿图，后者将用于回环检测。当检测到回环时，更新位姿图并优化位姿，使用优化后的位姿进行全局的语义地图构建。

Semantic Assisted ICP

基于语义信息对LOAM进行扩展：

1. 根据语义标签区分特征并且分别进行匹配。
2. 根据语义标签分别进行下采样，现有的方法多采用voxel滤波，但是这种滤波方法会丢失小物体的信息，本文对不同的物体采用不同的采样率。
3. 利用语义信息约束平面拟合。

步骤：

1. 使用LOAM中的特征提取方法，计算点的粗糙度，挑选出edge point和planar point。

2. 由于不同的语义类具有不同的含义，这有助于区分定位，所以基于语义信息给每个误差函数加上权重：

3. 用最近一段时间的特征点建立一个子图，当前扫描（滤波后）和子图进行匹配（滤波后）。对于当前帧的edge point，使用kd-tree寻找语义子图（具有相同的语义标签）中最近邻的几个点拟合一条直线，然后计算点到线的距离。对于当前帧的planar point，同样挑选语义子图中最邻近的几个点拟合一个平面（如果语义信息中表示是地面或者建筑物，在拟合使将平面方向作为先验信息），计算点到面的距离。

Semantic Based Loop Closure Detection

1. Loop Candidates Generation：挑选合适距离的帧作为回环的候选。
2. Similarity Scoring：使用本团队去年发表的一篇论文的方法计算回环的相似性。具体方法是，首先将语义点云聚类为语义图，并将其与对应的姿态一起保存在全局语义图中备用。由1产生的回环的候选输入到图匹配神经网络中打分，选择最高分的回环。
3. Geometric Verification：对2选出的回环进行验证，用候选回环点云附近连续几帧的扫描建立一个子图，并使用语义ICP匹配当前帧和子图，如果误差过大没救不保留这个回环。
4. Pose Graph Optimization：使用g2o建立图并优化。

实验结果

总结和思考

1. 本文使用语义信息来提升点云匹配的准确度，并设计了一个基于语义图的回环检测模块。

SSC: Semantic Scan Context for Large-Scale Place Recognition——浙大

主要工作——IROS-2021

1. 本文对基于激光雷达的场景识别提出了一个全局的描述子，该描述子能结合语义信息对3D场景有效编码。
2. 本文提出了一个两步的全局语义ICP方法来获得点云的3D位姿，该方法不需要任何初始值。
3. 本文用获得的3D位姿匹配点云来消除旋转或者平移误差对描述子相似性的影响。
4. 在KITTI数据集上对比了本文方法和流行方法的场景识别和位姿预测。

具体实现方法

之前有很多基于scan context的方法使用不完整的语义信息，并且忽略点云之间的微小位移；本文开发了完整的语义信息，并且强调了点云之间的微小位移也会对场景识别的准确率有很大影响。

原始点云经过语义分割得到两张语义点云$P_1,P_2$?，投影到XY平面然后经过两步语义ICP（第一步：快速计算yaw角，第二步：快速语义ICP），得到$(\Delta x,\Delta y,\theta )$。为了消除旋转和微小位移对场景识别的影响，用得到的位姿对点云$P_2$进行变换得到$P_a$，最后使用全局描述子生成$S_1,S_2$，并进行相似性打分。

Global Semantic ICP

Fast Yaw Angle Calculate

1. 首先从语义点云中挑选出具有代表性的物体（建筑物、树干、交通标志），然后将这些物体投影到xy平面，每个点具有已下信息。
   
2. 每个投影后的点云被划分为$N_a$个sector（扇形），只保留每个sector里离原点最近的点，最后得到两个只有$N_a$个元素的点云$P_{I1}$和$P_{I2}$。
3. 对点云$P_{I1}$和$P_{I2}$根据方位角进行排序，将极半径存储为向量$R_1$和$R_2$。
4. 类似scan context，对$R_2$进行旋转，找到与$R_1$?最一致的角度即为yaw角。

Fast Semantic ICP

1. 首先根据计算出的yaw角旋转$P_{I2}$?得到$P_{Ia}$。

2. 构建如下优化问题：
   

Semantic Scan Context

以前的scan context使用的是弱特征，比如高度和反射强度，本文提出的描述子将使用语义特征来表达场景。

Semantic Scan Context定义：

编码函数（挑选一个element中所有标签对应编码值的最大值）：

相似性打分：

实验结果

总结和思考

1. 本文主要提出了一个基于语义全局的描述子用于场景识别，并且提出了一个两步语义ICP用于获得3D位姿。
2. 本文还提到了可以使用获得的3D位姿给点云匹配提供初值，比如为NDT提供初值计算6D位姿。

OverlapNet: Loop Closing for LiDAR-based SLAM——波恩大学

主要工作——RSS 2020

1. 本文提出的方法可以在没有相对位姿的情况下预测两帧之间的交叠和yaw角。
2. 本文使用雷达里程计信息和交叠的预测来检测正确的回环候选。
3. 本文的方法可以提高当前流行的SLAM框架的位姿预测的准确率。
4. 本文解决了在没有先验位姿信息的情况下的回环检测。
5. 本文使用OverlapNet的预测结果作为ICP的初值提高帧间匹配的准确率。

具体实现方法

The Concept of Overlap

Definition of the Overlap between Pairs of LiDAR Scans

1. 和SuMa、SuMa++一样，先将点云数据通过球面投影成距离图像，称为vertex map。

2. 对于两帧点云数据$P_1,P_2$，先产生对应的vertex map$v_1,v_2$，然后根据位姿变换将$P_1$转换到$P_2$所在的位姿并投影成vertex map——$v^{,}$?。

3. 通过以下公式计算得$P_1,P_2$的交叠率：

但是在实时SLAM系统中，相对位姿往往不够准确，所以本文设计了一个神经网络来计算两帧之间的交叠率（不依赖相对位姿）。

Overlap Network Architecture

1. 神经网络的两条leg是完全一样的并且共享参数，每条leg是一个全卷积神经网络，最终输出的结果的尺寸为$1?360?128$。

2. Delta Head：预测两帧之间的交叠率，delta layer考虑了所有像素之间的不同性。
   

3. Correlation Head：用于预测两帧之间的yaw角。

Loss Functions

端到端同时训练，损失函数如下：

SLAM Pipeline

本文使用SuMa的框架，之修改了之前的回环检测的部分。

实验结果

以上均为作者阅读论文时的理解，本人能力有限，若有不对的地方，欢迎斧正。