CT-ICP：带回环检测的实时弹性激光SLAM

计算机视觉life”，选择“星标”

快速获得最新干货

标题：CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

：Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, and Franc?ois Goulette

编译：段逸凡 , 审核：方川，Li,Lei, 机构：PSL University

来源：arxiv

code：https://github.com/jedeschaud/ct_icp https://github.com/Kitware/pyLiDAR-SLAM

本文转载自泡泡机器人SLAM，文章仅用于学术分享。

摘要

大家好，今天为大家带来的文章是 CT-ICP: Real-time Elastic LiDAR Odometry wi th Loop Closure. 激光雷达被广泛运用于定位和感知任务中。本文提出了一个新的实时激光里程计方法，被称为CT-ICP，同时包含了回环检测功能。通过使用一种轨迹的弹性公式，考虑帧中数据的连续性和帧间数据的不连续性，以实现对高频运动的更鲁棒的目的。点云配准通过scan-to-map实现，使用稠密点云作为地图，并且通过基于稀疏体素结构的点云存储方式来获得实时的处理速度。同时，实现了一个使用高程图的快速的回环检测方法，来共同构成一个完整的激光SLAM系统。为了测试算法的鲁棒性，测试了七个数据集：KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College, NCLT. 数据集包含了驾驶和高频运动的场景。CT-ICP使用C++实现，并且开源；回环检测和位姿图优化被集成在了pyLiDAR-SLAM中，也进行了开源。CT-ICP是目前KITTI上有开源代码的算法中排名第一的（总第六），RTE为0.59%，平均运行时间为单核CPU 60ms。

图1 着色的点为雷达的一帧数据，颜色按照时间戳进行上色（蓝到红）；白色的点为地图。对每帧开始和结束时的两个pose进行联合优化，并且根据时间戳进行插值，从而形成连续时间scan-to-map的里程计。注：本文认为上一帧结束和下一帧开始时的位姿并不相同，即帧间的不连续性。 主要工作与贡献

• 一个新的弹性激光里程计算法，基于帧中连续性和帧间不连续性，并且为scan-to-map的配准

• 基于存储在稀疏体素结构中的密集点云的局部地图，以获得实时处理速度

• 在驾驶场景和高频运动场景中对7个数据集进行了大规模的实验

• 一个与位姿图优化集成在一起的快速回环检测方法

算法核心 CT-ICP

在CT-ICP中，一帧数据由开始时的位姿和结束时的位姿进行描述，分别用 和 表示。这一帧中的数据，在时间 间获得，其间每一时刻传感器的位姿由插值获得。位姿描述了从LiDAR坐标系 到世界坐标系 的变化。与其他的连续时间的轨迹方法不同， 与 并不相同。添加了一个接近的约束，来迫使两个位姿保持接近。这一操作使得对高频运动的场景更鲁棒。

对一个帧数据 ，首先从其中下采样出一个子集，其index记为 , 即 ，随后将其配准到local map上。local map为一个稠密的点云 ，由之前的 帧数据组成。通过配准，估计出 和 。使用这两个位姿将 合并到局部地图上。

和 通过下式进行求解，其中 涉及到的式子如下所示，具体解释在式子之后：

• Eq.1中的第一项 为scan-to-map continuous-time的ICP。

• 为点到平面的距离，如Eq.3所示，其中p,q分别为scan的点和map中对应的临近点， 为map点的法向量， 为根据平面平整度而设计的权重， , 为协方差矩阵的特征值。

• Eq.4描述了从 坐标系转到 坐标系， 为根据时间戳的插值操作。具体计算方法如Eq.5，Eq.6所示。

• 为loss function（huber， cauchy，truncated）。

• Eq.1中的第二项为位置一致性约束（location consistency constraint），即前文提到的使 与 接近，如Eq.7所示。

• Eq.1中的第三项为速度一致性约束（constant velocity constraint），限制了太快速的加速，如Eq.8所示。

• 和 为权重，实验中设为0.001。

局部地图

在本节，进行了一些操作来检测错误的配准。首先，局部地图以体素（voxel）的数据格式进行存储，每个体素中最多存储20个点，对于驾驶场景voxel的大小为1m,对于高频运动的场景0.8m。CT-ICP在点附近的27个（3x3x3）voxel中寻找最近的20个来进行配准。当旋转大于5度或有大量的点落在了空的体素中，则认为发生了错误的配准。算法将尝试以一组更保守的参数来重新进行配准。

回环检测和后端

图2 回环检测的示意图。左上为局部地图的高程图（elevation image）. 右上为效果展示。下方为回环检测的约束（绿色）。 回环检测部分整体思路为将局部地图转化为2D图像（elevation image），提取具有旋转不变性的图像特征。当检测回环时，通过与过往的elevation image进行对比来寻找候选。后端优化采用g2o. 实验

实验数据集分为两部分：驾驶场景，KITTI，KITTI-raw（没有运动补偿版本），KITTI-360（比KITTI更长）,KITTI-CARLA（仿真环境）,ParisLuco（32线）；高频运动，NCLT（两轮Segway采集，包含突然旋转场景），NCD（手持）。

图3 定量分析。

图4 回环检测的消融实验

独家重磅课程官网：cvlife.net

1、视觉SLAM基础： 刚看完《视觉SLAM十四讲》，下一步该硬扛哪个SLAM框架 ？

2、VIO灭霸：VIO天花板ORB-SLAM3第2期上线！（单/双目/RGBD+鱼眼+IMU+多地图+闭环）

3、机器人导航运动规划： 机器人核心技术运动规划：让机器人想去哪就去哪！

4、详解Cartographer： 谷歌开源的激光SLAM算法Cartographer为什么这么牛X？

5、深度学习三维重建 总共60讲全部上线！详解深度学习三维重建网络

6、三维视觉基础 详解视觉深度估计算法（单/双目/RGB-D+特征匹配+极线矫正+代码实战）

7、 VINS:Mono+Fusion SLAM面试官：看你简历上写精通VINS，麻烦现场手推一下预积分！

8、图像三维重建课程：视觉几何三维重建教程（第2期）：稠密重建，曲面重建，点云融合，纹理贴图

9、系统全面的相机标定课程：单目/鱼眼/双目/阵列 相机标定：原理与实战

全国最棒的SLAM、三维视觉学习社区↓

技术交流微信群

投稿、合作也欢迎联系：simiter@126.com

— 版权声明 —

本公众号原创内容版权属计算机视觉life所有；从公开渠道收集、整理及授权转载的非原创文字、图片和音视频资料，版权属原。如果侵权，请联系我们，会及时删除。

来源：SD科技制造