激光语义加入到激光回环。

《SA-LOAM: Semantic-aided LiDAR SLAM with Loop Closure》(ICRA2021)

Motivation

目前基于激光的定位效果已经很好了，但是基于激光的回环仍然是一个比较开放的课题(相关工作少), 因此作者在这篇论文中提出一个语义辅助回环检测的SLAM方法，效果不错。

Contribution

1. 提出了一个完整基于激光的语义SLAM，即使在超大范围场景，也可以构建一个全局稳定的语义地图。

2. 提出了一个基于loam的语义辅助ICP方法，通过语义信息提升了里程计的精度

3. 在系统里加入了一个基于语义信息的回环检测方法并且维护了一个全局的因子图(这个因子图里的节点是不同物体的语义信息)。

4. 加入了语义信息之后，在KITTI和FORD campus dataset的结果表明相比于原来的baseline的精度提升了很多。

   Content

5. 系统框架

   系统框架如下，原始点云P输入到语义分割模块来获得语义点云S，S并行输入给运动估计和回环检测模块，当检测到回环的时候，位姿图和全局语义图会进行更新，每一个局部语义图都会被保留用来构建全局的语义图。

1. 语义辅助的ICP

   这个模块可以理解为将语义信息加入loam点云注册模块，总体来说，改变有三点:

   a. 参考SE-NDT(端到端提取语义加入NDT)，在角点和平面进行注册匹配的时候，加入语义标签的限制，减少错误的匹配对。

   b. 根据语义标签来降采样角点点云和平面点点云。区别于目前流行的体素格过滤降采样方法(这个方法虽然速度快，但是会过滤掉很多低尺寸的语义目标)，文章中采用的方法是对不同的物体采用不同的降采样尺寸，最终可以保留很多低尺寸的语义信息，如下图。

   c. 利用语义信息来约束平面点拟合，基于地面平面应该平行与水平面或者垂直于建筑物面的假设，可以删除拟合比较差的平面。

具体的语义ICP流程如下:

a. 特征提取

loam那曲率那一套:

\[\text { roughness }=\frac{1}{n \cdot\left\|p_{i}\right\|}\left\|\sum_{j=1, j \neq i}^{n}\left(p_{j}-p_{i}\right)\right\|\]

b. 运动估计

残差大体上还是loam那一套，但是加入了一个基于语义信息的权重:

\[r=\sum_{l}^{L}\left\{\sum_{i}\left(w^{l} d_{e i}^{l}\right)+\sum_{j}\left(w^{l} d_{p j}^{l}\right)\right\}\\ d_{e i}^{l}=\frac{\left\|\left(c_{e i}^{l}-m_{e 1}^{l}\right) \times\left(c_{e i}^{l}-m_{e 2}^{l}\right)\right\|}{\left\|m_{e 1}^{l}-m_{e 2}^{l}\right\|}\\ d_{p i}^{l}=\frac{\left\|\left(c_{p i}^{l}-m_{p 1}^{l}\right)^{T}\left(\left(m_{p 1}^{l}-m_{p 2}^{l}\right) \times\left(m_{p 1}^{l}-m_{p 3}^{l}\right)\right)\right\|}{\left\|\left(m_{p 1}^{l}-m_{p 2}^{l}\right) \times\left(m_{p 1}^{l}-m_{p 3}^{l}\right)\right\|}\]

有意思的是，论文这一部分居然没讲权重，就是复述了下loam的残差公式？

c. 基于语义的回环检测

和大多数回环一样，分成四个部分: 回环参与者生成， 相似度评分， 机和一致性验证和位姿图优化

I:回环参与者生成:

就是为了减轻回环的工作量，只保留距离当前帧距离小于阈值的点:

\[\delta_{d}=\max \left(\delta_{\min }, \min \left(\delta_{\max }, \sigma d\right)\right)\]

II: 相似度评分

文章中简单的提了下用的是自己之前的工作: “Semantic graph based place recognition for 3d point clouds”( arXiv preprint arXiv:2008.11459, 2020).

III: 几何一致性验证

将回环候选帧和当前帧的邻近帧计算语义ICP误差，误差大于阈值则排除。

IV: 位姿图优化

每当找到一个回环，就用g2o优化一次， 误差公式:

\[e_{i, j}=Z_{i j}^{-1}\left(T_{i}^{-1} \cdot T_{j}\right)\]

1. 实验

主要通过实验证明了三点: 一是语义辅助的ICP可以有效提升里程计精度，二是语义辅助的回环可以有效减少累计误差，三是证明系统的鲁棒性:

Conclusion

这篇论文是将语义信息加入激光，前端里程计基本还是loam那套，但是加入了语义标签和语义权重，后段回环加入了语义的匹配，最终的效果很好。