中科院新作！即插即用的meshing模块！精确构建无动态mesh地图，还能纠正里程计！

点击下方 卡片 ，关注 「3DCV」 公众号
选择 星标 ，干货第一时间送达

编辑：3DCV

添加小助理：dddvision，备注：方向+学校/公司+昵称，拉你入群。文末附3D视觉行业细分群。

扫描下方二维码，加入 3D视觉知识星球 ，星球内凝聚了众多3D视觉实战问题，以及各个模块的学习资料： 近20门视频课程（星球成员免费学习） 、 最新顶会论文 、计算机视觉书籍 、 优质3D视觉算法源码 等。想要入门3D视觉、做项目、搞科研，欢迎扫码加入！

0. 作者有话说

这项工作最开始是收到了SLAMesh的启发，当时复现这篇论文的时候，感觉它的工作做得很扎实，论文写的好，创新点新颖，github上readme写得很详细，怎么评估地图都写了，然后代码写的也很优雅。复现完之后，发现它构建的地图有个主要问题就是没有剔动态，比如kitti07序列上，路口处全是动态拖影，导致最后地图很丑(在后面实验部分会看到)，然后就想到能不能给mesh做个 剔动态的方法 。

另外，还有一个新奇的想法就是，现在大部分工作更多关注里程计，比如最近的KISS-ICP，它们主要是提高里程计定位精度，用的还是点云地图；另一些研究者在做建图，但是这种建图不是SLAM中说的后端，一般后端都要纠正一下里程计精度，而这些建图工作都默认输入的位姿是精确的，甚至直接用真值作为输入，这就没考虑到里程计漂移对建图的影响。于是就想能不能做一个 即插即用的SLAM中的mapping模块 ，让这些里程计和我很方便的集成，然后能做到精确的定位和建图。于是产生了这样一个即插即用的mapping模块的想法。

1. 效果展示

这是我们的算法在KITTI07上构建的地图，几个细节放大可以看到，在具有动态的几个路口都能实现比较好的动态剔除和建图效果。

代码打算开源，目前正在整理代码，欢迎大家评论交流：https://github.com/Yaepiii/CAD-Mesher

项目主页：https://yaepiii.github.io/CAD-Mesher/

2. 论文信息

标题：CAD-Mesher: A Convenient, Accurate, Dense Mesh-based Mapping Module in SLAM for Dynamic Environments

作者：Yanpeng Jia, Fengkui Cao, Ting Wang, Yandong Tang, Shiliang Shao and Lianqing Liu

机构：中国科学院大学、沈阳自动化研究所

原文链接：https://arxiv.org/abs/2408.05981

代码链接：https://github.com/Yaepiii/CAD-Mesher

官方主页：https://yaepiii.github.io/CAD-Mesher/

3. 摘要

大多数雷达里程计或SLAM系统都是在点云中构建地图，这是一种离散且放大后稀疏的地图，并不能直接用于导航。mesh地图是一种密集且内存消耗少的地图表示形式，它可以用简单的基本元素近似复杂的形状，因而近年来受到研究者的广泛欢迎。然而，它们大都在静态环境假设下运行。实际上，当环境中存在移动物体时，可能会留下拖影，导致建图质量下降。在本文中，我们提出了一个适用于动态环境的即插即用的mapping模块，该系统可以很方便地与各种雷达里程计融合构成SLAM系统，可以在过滤动态物体的影响增量式地构建精确、一致且稠密的网格地图的同时，利用mesh的优势有效地提升里程计的位姿估计精度。在我们的meshing模块中，一个新颖之处是使用所提出的两阶段由粗到精的动态剔除策略缓解移动物体的影响。据我们所知，这是第一个具有显式动态剔除的mesh构建方法。我们使用基于滑动窗口的关键帧聚合以及自适应降采样保证点云均匀分布，然后基于高斯过程进行网格构建。我们在大量公开数据集上评估了定位和建图精度。定性和定量的结果证明了我们的方法优于最先进的算法。

4. 算法解析

该系统作为一个SLAM中的mapping模块，接收t时刻雷达坐标系L下的原始点云和里程计估计的雷达坐标系L到全局坐标系O的位姿变换作为系统输入。

1)自适应关键帧选择和自适应降采样： 首先使用自适应选择机制，选取关键帧。这主要是降低数据的冗余，提高覆盖范围和系统效率。主要思想是考虑到在大规模的环境中，点云分布更加分散，划分在体素内的点可能不足或不确定性较高导致网格重建精度较低甚至无法进行重建；相反，在狭窄或小规模的环境中，可以选择较大的阈值丢放关键帧，因为这里点云比较密集，每个体素内有足够多的点。于是，我们定义了一个“空间性”的概念选择关键帧阈值

在进行基于range图像的动态粗剔除后，加入基于滑动窗口的关键帧数据库。然后将窗口内的关键帧进行聚合后变换到世界坐标系W， 。为提高系统效率，我们提出了一种自适应降采样策略对聚合后的点云进行均匀采样。与上文类似，若“空间性”较大，则使用小尺寸的降采样保留更加密集的点；相反，当周围空间比较逼仄时，采用较大尺寸的降采样可以在加快运算速度的同时，保留环境的细节。

2）基于高斯过程的mesh建图： 常规的高斯过程通常输入所有点，并假设所有点的误差为各向同性，这意味着每个点都是被同等对待的。然而我们注意到，一些孤立细窄物体(鼓励的树枝、杆子)、嘈杂部分(叶子、灌木)以及离群点，可能会在前后帧之间存在不一致性，这可能会影响建图精度，并进一步影响point-to-mesh配准质量。

这一步用的还是类似于曲率的概念。然后进行高斯过程mesh建图，这一步类似于SLAMesh，但通过上述的一些策略，能保证建图精度更高，高斯过程输入的训练点更精确。详细过程可以参考论文，这里就放一下预测值和协方差：

增量更新过程就是用最小二乘法更新：

3）两阶段由粗到精动态移除： 由于使用关键帧累计策略，需要处理的点的数量将会增加。为了在保证系统运行效率的同时，降低动态物体对配准的影响。因此，我们建议先使用基于可见性的方法进行快速动态剔除， 这一步的主要目的是提高point-to-mesh的质量 ，因为mesh是静态的，点是动态的，这会造成不一致。首先将上一时刻构建的聚合点云变换到当前时刻(数据复用)，然后进行球面投影，range图作差，如果差值大于一定阈值就认为是动态的。

我们称这一阶段的动态剔除为粗剔除，因为该方法的性能依赖于选定的像素分辨率。如果分辨率设的太大则会过度剔除静态点，降低网格重建质量；而如果分辨率设的太小，动态剔除不干净，将会影响配准精度。因此，我们在这选择保守的分辨率，在后端再使用基于体素的概率方法进行第二阶段动态剔除，精细剔除剩余动态点。

精动态剔除步骤在point-to-mesh配准步骤之后，因此我们已经获得了更精确的位姿变换。传统方法通常采用光线追踪更新每个体素的占用概率，以判断动态物体。尽管经过八叉树加速搜索，这一过程仍然非常耗时。我们的方法采用类似可见性检查的方法替代这一过程。

在这一阶段，我们首先检查当前帧与全局地图的重叠体素，这些体素的被认为是“占用”，同时，其位置通过编码存储为体素的哈希索引。然后我们对周围体素进行遍历，并对其索引进行解码(在进行体素索引编码时用了一个小trick将位置编码进索引，这样能很方便的解码)，以此计算这些体素与机器人坐标原点的距离，只要这个距离小于当那些覆盖体素距离的都认为是“空闲”。然后我们使用贝叶斯方式对每个体素的占用概率进行更新：

在代码中是用log-odds的方式实现的。若某一体素的占用概率高于阈值 则认为是稳定静态物体，将会保留在最终的网格地图中；相反，若占用概率低于阈值 则认为是空闲区域，删除其中内容。

通过这种方法代替光线追踪，可以在过滤剩余动态物体的同时，加快计算效率，同时这种方法可以 通过多线程进行加速 。

从下图可以看出仅使用其中一种方法仍然会在地图中留下不少动态物体的拖影。而使用我们提出的阶段由粗到精动态剔除策略得到的地图基本完全过滤了动态物体，得到了更加干净的网格地图。

最后就是point-to-mesh配准，我们和Puma不一样，不是采用ray-casting的方式，而是直接最近邻搜索，搜索到最近顶点后，沿着连接进行搜索附近定点，然后进行法线平均，然后就可以构建残差：

使用mesh的优势主要在于，mesh是平滑且连续的，可以从效果图看出，比如地面和墙面这些部分是非常平滑的，这部分的法线估计肯定比比如FAST-LIO中那种用最近几个点拟合的平面求法线更精确，因此point-to-mesh能获得高质量的定位精度。另外，这里使用里程计求解的结果作为ICP先验输入，一定程度上能缓解进入局部最优解的问题。

5. 实验

1）Meshing定量评估： 在两个经典meshing数据集上评估了我们的方法，MaiCity是模拟数据集，距离阈值设置为0.1m，NewerCollege是真实数据集，距离阈值设置为0.2m。

在两个数据集中，我们均取得了最高的重建精度，但召回率均不如SHINE-Mapping。我们认为可能是由于我们的粗动态移除策略错误删除了部分静态点，同时一致性检查在过滤噪声点的同时，误删了一些细长的杆(如树干、树枝、电线杆)。但综合来看，我们的方法仍取得了较高的F1分数，这证明了我们方法的有效性。此外，值得注意的是SHINE-Mapping是一种基于深度学习的离线后处理的方法，需要长时间的训练并且无法实时运行。

2）Meshing定性评估： 定性评估主要看一下动态剔除和应对稀疏雷达的效果，在kitti07和我们自己收集的16线雷达数据集上测试了效果。

动态效果一眼可见就不多说了，稀疏雷达提出的挑战一个是点云稀疏的情况下一些mesh方法可能会产生孔洞，影响了mesh地图的连续性(如VDBFusion和SLAMesh)；另外就是，稀疏雷达情况下，里程计也容易产生漂移，SHINE-Mapping这种离线建图方法，它是不考虑里程计漂移的，因此建出的图来产生了分层的现象，而我们的方法纠正了里程计漂移因此显示出了更好的一致性。

3）定位评估：

可以得出以下几个结论：

• 各种雷达里程计在与我们的CAD-Mesher模块进行拼接后，可以使得定位精度得到进一步的提高。并且优于传统的Mapping模块。
• 我们还注意到某一算法可能原本表现不如另一个，但与我们的mapping模块集成后，显示出比其更优秀的精度；而带有我们模块的后者则又能超越带有我们模块的前者（如KITTI00的DLO和KISS-ICP）
• 在高动态场景下，带有我们方法的里程计可以实现非常大的性能提升，其中A-LOAM在UrbanLoco05序列失效，但与我们的CAD-Mesher集成后，可以得到超越其他方法的精度。我们认为这归功于我们高效的两阶段由粗到精动态移除策略。此外，我们的方法比动态SLAM方法能更好的应对动态情况。
• 即使不使用雷达里程计估计的位姿作为先验，我们的方法也实现了具有竞争力的性能。普遍超过了其他里程计。
• 我们还注意到一个有趣的现象，基于点云配准的里程计与我们的基于mesh的方法集成后带来的提升普遍超过带有我们方法的基于mesh的里程计。我们分析认为可以是基于点云的配准与我们的point-to-mesh配准解耦，起到了互补的作用。

4）消融： 建图质量是MaiCity上评估的，由于这个数据集没有动态，定位精度是在kitti07上评估的。

根据(a)-(d)，随着滑动窗口尺寸的增加，建图精度和完成度均有增加。同时，由于用于配准的点的数量和范围增加，里程计精度也有相应的提升。这体现了我们滑动窗口和关键帧机制的优越性。但若过度增加滑动窗口的尺寸，虽然会带来精度提升，但却会增加更多的计算时间。

此外，通过比较(c)、(e)、(g)，可以看到，如上所述，使用连续性测试和动态去除策略对位姿估计精度有十分积极的作用，但由于一些误删，会造成地图质量的轻微下降。然而，我们致力于实现一个便捷、精确、稠密的基于mesh的SLAM中的Mapping模块，而非离线地图生成器。因此，综合权衡下，我们认为我们的一致性检查和动态移除策略是有效的。

最后，根据(c)和(f)，可以看到，通过使用自适应降采样，系统的计算时间减少了将近一半，而并没有带来过多建图精度的损失。这体现了我们自适应降采样策略在时间消耗方面的优势。

5）时间消耗： 常规的mapping模块仅要求1Hz以上的运行频率，我们的系统在64线雷达情况下仍然可以达到8Hz的运行效率，并与雷达里程计融合后发布精确的实时位姿估计。此外，在32或16线雷达情况下我们的方法可以做到10Hz以下实时运行。

6. 总结

在本文中，我们提出了一个即插即用的mapping模块，它可以与各种雷达里程计方便地进行拼接，得到更精确的位姿估计和一致、高质量且干净的地图。它的核心是基于滑动窗口的关键帧聚合策略以及两阶段由粗到精动态移除策略，这为高质量的网格重建奠定了良好的基础。在多个公开数据集下的实验证明了我们的观点。

本文仅做学术分享，如有侵权，请联系删文。

3D视觉交流群，成立啦！

目前我们已经建立了3D视觉方向多个社群，包括 2D计算机视觉 、 最前沿 、 工业3D视觉 、 SLAM 、 自动驾驶 、 三维重建 、 无人机 等方向，细分群包括：

工业3D视觉 ：相机标定、立体匹配、三维点云、结构光、机械臂抓取、缺陷检测、6D位姿估计、相位偏折术、Halcon、摄影测量、阵列相机、光度立体视觉等。

SLAM ：视觉SLAM、激光SLAM、语义SLAM、滤波算法、多传感器融合、多传感器标定、动态SLAM、MOT SLAM、NeRF SLAM、机器人导航等。

自动驾驶：深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器、多传感器标定、多传感器融合、自动驾驶综合群等、3D目标检测、路径规划、轨迹预测、3D点云分割、模型部署、车道线检测、Occupancy、目标跟踪等。