RAL2023开源I基于多输入Surfel的激光惯性连续时间里程计和建图
作者：敖丙 | 来源：3D视觉工坊
在公众号「3D视觉工坊」后台，回复「原论文」可获取论文pdf和代码链接。
添加微信：dddvisiona，备注：SLAM，拉你入群。文末附行业细分群。
虽然与全局地图的特征关联有很大的好处，但为了防止计算呈指数级增长，大多数基于激光雷达的里程计和建图方法都选择将特征与一个体素尺度的局部地图关联。不同体素尺度的曲面可以组织成树状结构，因此我们提出了一种基于八叉树的多尺度曲面全局地图，该地图可以增量更新。这减小了重复计算整个地图的k-d树的耗时。该系统还可以从单个或多个传感器获取输入，从而增强退化情况下的鲁棒性。我们还提出了一种点到表面（PTS）关联方案，对PTS和IMU预积分因子进行连续时间优化，以及闭环和束调整（BA），提供了一个完整的激光雷达惯性连续时间建图定位框架。在公共和内部数据集上的实验表明，与其他最先进的方法相比，我们的系统具有优势。这里也推荐「3D视觉工坊」新课程《彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战》。
一个完整的激光雷达惯性里程计和测绘框架，具有前端里程计、闭环和全局姿态图优化，能够集成多个激光雷达输入。
一种多尺度点对面（PTS）关联策略以及滑动窗口上PTS和IMU预积分因子的连续时间优化。
基于UFOMap框架的分层多尺度surfel地图的实现，实现了全局地图的增量更新和高效查询。
在公共和内部数据集上进行了大量实验，以验证多尺度surfel关联策略的结果和效果。
系统基本框架
首先是将来自不同传感器的数据进行同步，然后进行IMU传播、激光雷达数据运动补偿。其次，执行scan2map的PTS关联匹配、IMU预积分，构建残差调用ceres进行图优化、边缘化，若发生回环则添加回环约束并更新地图。
surfel 和 UFOMap
surfel属性
如图所示，每个点集都是一个surfel，持有各自的属性；基于八叉树的UFOMap中，一个节点i对应一个体素（voxel）,节点深度表示体素尺度。叶子节点深度是0,其父节点深度是1,依次类推。节点i中的一个曲面（surfel)定义：包含在该体素内的点集；
每个surfel中存储: ,分别表示surfel中点数、点集、协方差分布相关。基于这些属性，可以快速计算得到重心、协方差分布、类平面度、法向量、重心到平面的距离。
surfel的添加和删除（新关键帧加入后，surfel map更新方式）
假设父节点包含子节点和，则i的surfel属性计算方式：
如果节点被删除，则的属性更新方式为：
即使增量更新的点云中数量只有一个点，上述公式依然可用，此时.
PTS关联匹配
主要分为两步：
1).对于每个激光点，在surfel map中找到所有匹配的节点（每个节点都是一个体素）；
具体匹配条件如下：
a. 体素深度在1和之间（叶节点忽略）；
b. 且即 应至少具有个最小点，并且平面度足够大.
c. 体素的立方体与以为中心半径为的球体相交，其中是用户定义的参数。
2).计算激光点到的距离。
如果距离小于阈值,则将一组PTS系数将被添加到成功关联的集合中，然后用于在优化阶段构建代价函数。
PTS系数：
激光匹配残差计算公式：
图优化、闭环
残差包括：IMU预积分残差、激光PTS关联匹配残差、回环约束、帧间约束。
需要注意的是，优化使用的是滑窗策略，回环结束后需要重构局部地图。
边缘化
在每次优化之后，检查姿态估计结果是否可以被边缘化以成为关键帧。为此，在现有关键帧中搜索的五个最近邻居。如果到其所有邻居的欧几里得距离或旋转距离超过阈值，则其相关的去畸变点云将被插入到surfel map中。然后将边缘化，并将其去畸变的点云存储在缓冲区中，以便在闭环的情况下重用。
数据集：NTU VIRAL Dataset, Newer College Dataset，In-house Dataset
对比方案：MARS，LIO-SAM，FAST-LIO2
表1提供了NTU VIRAL数据集上的性能表现（rmse误差）：SLICT < FAT-LIO2 < MARS/LIO-SAM，MARS和LIO-SAM轨迹会出现发散的情况。
SLICT目前无法保证实时性，如果使用多线程耗时可能有所改善，两次优化之间的耗时接近160ms，其中优化求解耗时接近50ms，其余耗时部分是去畸变、特征关联以及关键帧边缘化。这里也推荐「3D视觉工坊」新课程《彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战》。
实验参数设置与NTU VIRAL数据集中的设置类似，但对于SLICT，我们为每个间隔分配2个状态估计量。这是主要因为数据集的IMU，即Ouster激光雷达的内置IMU，频率仅为100Hz。（请注意，LIO-SAM在这种情况下无法运行，因为它需要IMU的方位估计。)
由于ATV的高速（高达30km/h），MARS在所有三个实验中都会发散，LIO SAM在最后一个实验中也会发散。在没有闭环的情况下，SLICT仍然具有最佳的精度，并且闭环进一步提高了精度。我们还添加了LIO-SAM的带回环的测试结果，使之能够与SLICT进行比较。
由于环境的高程变化很大（最高点和最低点之间的高度高达15m），定位误差也很大，在z方向维度上最明显。与图中 NewerCollege数据集相比，我们可以看到z方向的误差现在几乎翻了一倍。
在本文中，我们提出了一个完整的多输入激光雷达惯性测距和测绘系统，称为SLICT。SLICT的特点是可以使用UFOMap框架进行增量更新的多尺度全局surfel map。我们还提出了一种将激光雷达点与曲面（PTS）相关联的方法，以及PTS和IMU预积分因子的连续时间最大后验优化。我们在公共数据集上展示了有竞争力的性能，即使不开闭环。为了实现一个完整的系统，我们添加了一个简单而有效的闭环机制，并用我们的新数据集证明了它的有用性。为了社区的利益，我们发布了源代码。SLICT的扩展仍有许多可能性。首先，通过一些自适应策略可以提高PTS关联的效率。此外，UFOMap框架允许将语义信息集成到surfel中，这有可能改进关联过程。对于小漂移，基本的闭环机制就足够了，但未来也可以集成更先进的方法。
