近日我们团队参加的 IROS 2021 The HILTI SLAM 挑战赛获得了最终的冠军。本文是对于本次比赛的算法的实验和思考，版权所有：旷视科技。原文链接：https://zhuanlan.zhihu.com/p/420192586

近日，IROS 2021 The HILTI SLAM 挑战赛公布了竞赛结果，旷视研究院荣获此次挑战赛的冠军。这是继去年包揽 CVPR 2020 SLAM 挑战赛全部赛道的冠军后，旷视研究院再次获得顶会 SLAM 竞赛的冠军。

2021 HILTI SLAM 挑战赛是国际顶级会议——智能机器人与系统国际会议（IEEE/RSJ International Conference on Intelligent Robots and Systems，以下简称IROS）中的一项竞赛，由苏黎世理工和 HILTI 联合举办。IROS 成立于 1988 年，每年举办一次，已发展成为全球规模最大、影响力最强的机器人研究会议之一。

1 赛题介绍

实时定位与地图构建（Simultaneous Localization and Mapping，SLAM）是机器人导航和执行任务所必需的基本能力之一。虽然近些年来基于几何的方法和基于学习的 SLAM 方法都取得较大的进展，但在真实世界中部署一套精准且鲁棒的SLAM算法，仍然是一个具有挑战性的问题。在这次挑战赛中，HILTI 提出了一个新的数据集，包含 12 个序列。每个序列都代表不同阶段的施工现场，如地下室、校园、建筑工地、办公室、实验室、露天停车场等。参赛队伍可以使用数据集中提供的任何传感器设计 SLAM 算法。挑战赛主办方会根据参赛队伍轨迹的完整性和精度进行评分，并按照总分的高低进行排名。

2 旷视夺冠算法揭秘

本次比赛吸引了全球20支企业和高校队伍参赛。最终，我们采用基于双激光+IMU 的 SLAM 算法作为参赛方案，经过激烈的竞争最后获得了竞赛冠军。

获奖证书：

Leader Board：

我们分析了比赛数据集提供的传感器信息。其中视觉模块为 SevenSense 的五目相机模组，该模组能够提供丰富的纹理信息，但对光照变化比较敏感，特别是在室外环境。而 3D 激光传感器在不同光照下均能够提供鲁棒和精确的距离测量，更适合该数据集。另外将这5个相机进行融合工作量较大，短期内无法获得一个满意的结果。因此，我们选择 3D 激光雷达作为主要的传感器。

数据集中提供了两个激光雷达，其中 Ouster 具有360度的视场角，能够提供稠密的测量；而 Livox MID70 的视场角仅有70度，但它的测量范围长达260米。使用单一的激光雷达容易出现退化的情况，即使利用 IMU 能够保证短时间内的精度，但整体的定位精度会受到影响。如下图（左），仅使用 Ouster 激光，在空旷的环境中能够定位的特征很少；下图（右），仅使用 Livox 激光，在特定数据集中视野受限。因此，我们最终选择了双激光+IMU的方案。

整体方案如下，这是一个基于滤波的激光惯导里程计：主要分为数据同步、畸变补偿、滤波器的预测更新、地图管理等步骤。

实际数据中，两个激光并不是完全同步的，时间戳相差 10-20ms。因此第一步是将 Livox 的激光点云按照每个点的时间戳重新组织（即切割 Livox 的点云数据），与 Ouster 的时间戳进行对齐。接着在 Ouster 一帧激光的测量时间内，利用 IMU 测量进行状态更新。状态更新的数据可以去点云的运动畸变。然后将 Livox 去畸变的点云转换到 Ouster 坐标系下，与 Ouster 的激光点云进行合并。数据同步步骤尤为关键，大大简化了程序的处理流程。

在滤波器更新阶段，采用点面距离构建测量模型进行更新。其中，每个点对应的平面是根据 K 近邻搜索和 PCA 拟合平面获得的。在获得优化的位姿后，将当前帧的点云转换到全局坐标系下并插入地图中。由于数据集的场景并不算太大（最大的场景为 100*100 米），同时为了程序的效率，我们采用一个增量式的 KD 树维护全局地图。为了保持全局地图的稀疏性，只有离地图中已存在的点较远的点云才可以插入。

3 亮点总结

1 双激光的结合大幅度地提升了程序的鲁棒性和精度；
2 滤波器能够实时估计出IMU的偏置和速度，能够提升畸变补偿的精度，反过来提升定位建图的精度；
3 由于全局地图的存在，不需要回环模块。程序得到了简化。

最后为大家展示一下利用算法构建的地图：

参考文献

1. Helmberger, Michael, et al. "The Hilti SLAM Challenge Dataset." arXiv preprint arXiv:2109.11316 (2021).
2. Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry." arXiv preprint arXiv:2107.06829 (2021).
3. Zhang, Ji, and Sanjiv Singh. "LOAM: Lidar Odometry and Mapping in Real-time." Robotics: Science and Systems. Vol. 2. No. 9. 2014.
4. Hackel, Timo, Jan D. Wegner, and Konrad Schindler. "Fast semantic segmentation of 3D point clouds with strongly varying density." ISPRS annals of the photogrammetry, remote sensing and spatial information sciences 3 (2016): 177-184.
5. Cai, Yixi, Wei Xu, and Fu Zhang. "ikd-Tree: An Incremental KD Tree for Robotic Applications." arXiv preprint arXiv:2102.10808 (2021).
6. Mourikis, Anastasios I., and Stergios I. Roumeliotis. "A multi-state constraint Kalman filter for vision-aided inertial navigation." Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007.