Home / 机器人经典基础 / KISS-ICP：把简单基线做好，究竟能走多远

100｜KISS-ICP：把简单基线做好，究竟能走多远

论文：Ignacio Vizzo、Tiziano Guadagnino、Benedikt Mersch、Louis Wiesmann、Jens Behley、Cyrill Stachniss，KISS-ICP: In Defense of Point-to-Point ICP—Simple, Accurate, and Robust Registration If Done the Right Way，2022 年预印本，2023 年发表。原文 v2 · 官方代码。

先接住前作的问题

ICP奠定最近邻与位姿交替估计，但缺少好初值时容易停在错误解；KISS-ICP 直接承接点到点配准，用连续运动预测、采样和自适应门限补齐里程计条件。它与FAST-LIO2是不同传感器信息量的平行路线，也与TEASER的全局鲁棒对应问题互补；系列到此应比较新增能力和代价，而非把简单等同于万能。

五个前置概念

五个前置概念：ICP交替更新对应与变换；鲁棒核限制异常残差影响；恒速模型用上一小段运动预测下一段；体素采样按空间密度保留点；局部收敛只说明初值附近优化稳定，不能保证全局正确。

一、问题设定：经典点到点ICP怎样在连续扫描中形成可靠里程计

两个点云一对齐就偏了，原因可能不是损失函数不够先进，而是输入畸变、初始运动预测差、对应距离阈值不合适，或者地图采样不均。把这些问题都留着，再增加复杂组件，未必能得到稳定系统。

背景：连续扫描和任意配准不同

拿两个完全陌生、相隔很远的点云做全局配准，必须解决大范围初始化问题。机器人连续扫描则通常拥有前一时刻运动估计。可以先预测“这次大致在哪里”，再让 ICP 求剩下的修正量。

输入为按时间到达的点云，运动补偿还需要逐点采样时间。状态输出是位姿序列，地图保存局部几何。这里没有学习策略、奖励函数或训练集拟合；实验是在固定算法与参数下处理传感器序列。

这并不意味着连续数据总容易。若丢帧造成时间间隔突然变长，或设备在一帧内快速加减速，前一帧速度就可能失去预测价值。判断输入是否满足局部配准条件，比单看算法名称重要。

二、围绕预测误差安排去畸变采样和自适应阈值

运动预测和去畸变

假设 $T_{t-2},T_{t-1}$ 为连续位姿，在近似相同采样间隔下，可把上一步相对变换作为下一步预测：

$\Delta T_{pred}=T_{t-2}^{-1}T_{t-1}, \qquad \widehat T_t=T_{t-1}\Delta T_{pred}.$

“匀速”是一个局部近似，不是对整条轨迹恒速的假设。机器人可以转弯和加速，只要短时间预测还足够接近，几何配准就有机会补上偏差。若帧间隔变化，上式需要按时间解释，不能无条件复制同一个位移。

将扫描内每个点按其相对时间补偿到共同参考后，再执行配准。没有 IMU 不等于没有运动补偿；这里运动来自已有位姿。也不等于没有时间要求：点自身的时间和扫描起终时间仍必须有意义。

两种密度服务两种任务

登记到地图的点云可以较密，参与当前求解的点云可以更稀。前者为未来查询保留空间覆盖，后者控制当前计算负担。体素采样保留真实观测点，有助于避免用格子中心替代实际表面造成离散化偏移。

从统计角度看，下采样也改变了权重：一块近处墙面若有数万个点，远处角落只有几十点，直接使用全部点可能让墙面支配目标。空间均衡采样不仅是加速操作，还会影响哪些几何区域决定变换。

为什么对应阈值应该随运动预测误差变化

若预测已经非常准确，过大搜索范围会引入错误对应；预测误差较大时，过小范围又会把真实对应全部拒绝。论文 §III-D 用已观察到的运动修正估计阈值尺度，而不是每个场景都重新手调固定距离。

其中的几何关系可独立推导。一个距离不超过 $r_{max}$ 的点，因旋转误差 $\theta$ 与平移误差 $\Delta t$ 造成的位置偏差满足：

$\|\Delta Rp+\Delta t-p\| \le 2r_{max}\sin(\theta/2)+\|\Delta t\|.$

旋转项是圆上的弦长上界，平移项来自三角不等式。同样一度的角度误差，在远处点上会变成更大的米制位移；因此阈值不能只看平移速度。

论文从历史修正的平方平均构造尺度，排除过小修正后用于匹配门限。这里的平方均值是以零为参考的量，不是先减去样本均值的标准样本方差。把它解释成经过严格概率校准的置信区间，会超出这一实用启发式的证据。

这个尺度统计可以用几项修正量验算。原创教学例： 历史有效运动修正的位移上界都是 0.2 m，并且均超过原文的最小纳入阈值 0.1 m，则 $\sigma=\sqrt{(0.2^2+0.2^2+0.2^2)/3}=0.2$ m，对应门限 $\tau=3\sigma=0.6$ m；若误用减均值的标准差，会算成零，从而把对应搜索范围错误压缩。若随后很久都是零修正，它们不进入有效集合，已有尺度也不会被大量零样本稀释。

再令旋转预测误差为 $1^\circ$ 、平移误差为 0.05 m。距离上限 10 m 的点，位移上界约为 $20\sin(0.5^\circ)+0.05\approx0.2245$ m；距离上限 100 m 则约为 1.7953 m。相同角度误差的远点可能越过 0.6 m 门限，说明有效量程和运动预测共同决定门限是否合理，不能仅观察车速。

原文III-D式(9)–(10)用的是此前轨迹中通过阈值的修正集合，不是自动遗忘旧数据的短滑窗。优点是避免静止阶段过度收紧；代价是旧的大修正会继续影响统计。这样的“3倍尺度”是操作性规则，其输入是非负几何上界、还经过阈值筛选，不足以直接宣称真实对应具有某个固定高斯置信覆盖率。

鲁棒点到点目标

在当前对应集合 $\mathcal C$ 上，通用写法为：

$\min_{R,t}\sum_{(p,q)\in\mathcal C}\rho\bigl(\|Rp+t-q\|\bigr).$

点到点残差不需要法向估计，减少了局部法向在稀疏或噪声数据下的不稳定来源。论文使用鲁棒核降低大残差影响，并迭代更新对应与位姿。鲁棒核和距离门限作用不同：门限决定哪些对应进入优化，核函数决定进入后的相对影响。

ICP 仍是局部方法。最近点不一定是真实同一物理点，对应关系随估计更新也可能跳变。小的最终增量可能表示找到好解，也可能表示停在一个错误的局部稳定位置。

预测、采样和门限共同决定进入ICP的对应分布。下面沿组件逐项检查，再把去畸变能力与已处理输入上的轨迹结果区分开。

三、运行局部地图配准，逐项检查基础组件

输入：连续点云及逐点时间，历史位姿和局部地图
从最近运动估计当前位姿与扫描内运动
按逐点时间做去畸变
得到较密的地图更新点集和较稀的配准点集
从历史预测修正统计匹配距离尺度
以运动预测为初值反复：
    搜索局部地图近邻，拒绝超过门限的对应
    求鲁棒点到点位姿修正
    更新当前位姿，检查修正是否足够小
记录实际运动相对预测的偏差，供未来阈值更新
将地图更新点集变换入地图，限制体素容量和局部范围
输出位姿及地图

这是模块级伪代码。原文使用增量收敛条件，未用固定最大迭代数作为其停止策略；工程系统若另设超时，应同时定义超时后的失败状态，不能把被中断结果当成正常收敛。

四、按原始或已去畸变输入拆开评测

Tables V–VI：上表是 KITTI-raw 上不同去畸变方法的比较，同时列误差与频率；下表改变关联门限，列相对平移误差 — 原论文结果图／表摘引：Ignacio Vizzo、Tiziano Guadagnino、Benedikt Mersch、Louis Wiesmann、Jens Behley、Cyrill Stachniss（2022），Tables V–VI，PDF 第 7 页；核对原文。原版权保留。

读图：上表是 KITTI-raw 上不同去畸变方法的比较，同时列误差与频率；下表改变关联门限，列相对平移误差。恒速补偿在该协议有效，自适应门限的收益主要是省去逐场景挑参数，不能夸大成数量级精度提升。

实验：已经去畸变的数据不能验证去畸变

这里对应论文 v2。已去畸变的 KITTI 输入不能验证本系统自己的去畸变模块，后续代码默认参数也不能直接代替原实验配置。

检查项	原文设置与含义
基准	KITTI、MulRan、Newer College、NCLT 等不同平台数据
KITTI 主比较	使用已补偿点云，对相关方法关闭再次运动补偿
运动补偿消融	改用 KITTI raw，才能比较有无去畸变
基线	里程计与带回环的系统分开标注，不能忽略额外后端
参数	表 I 列统一参数；最大量程仍与传感器及场景有关
指标	相对平移/旋转误差、部分 ATE 和处理频率
重复	MulRan 的多次采集平均不等于同一序列多随机种子重复
未确认项	全部随机种子、调参预算与各设备尾部延迟未逐项核实

三组核心结果分别说明：

表 II：主基准。 KITTI 00–10、11–21 的平均相对平移误差为 0.50%、0.61%。它支持一个简单里程计具有竞争力；该表不是证明本方法去畸变能力的实验。
表 V：输入畸变消融。 在 raw 数据上，不去畸变为 0.91%，恒速补偿为 0.49%，IMU 补偿为 0.51%。这说明在该协议下简单预测很有效，不说明 IMU 在所有运动下都多余。
表 VI：门限消融。 平均自适应误差 0.50%，几个固定门限约 0.51%–0.53%。主要意义是减少逐场景挑门限；提升幅度不能夸大成性能数量级变化。

还要保留失败边界：表 IV 的 Newer College 长序列中，KISS-ICP 为 0.96%，CT-ICP 为 0.58%，原文解释涉及后者额外回环。作者也明确指出 NCLT 真值和缺帧问题，因此该数据上的精确排序应谨慎看待。

一个自构实验与五个前置概念

未运行实验：固定同一原始序列，逐步增加丢帧间隔，比较固定门限和自适应门限。记录预测误差、有效对应数、最终漂移、迭代数和处理延迟。再加入一段长时间静止，检查门限是否缩得过小。这个实验能检验阈值机制对“历史误差可以预测未来”这一假设有多敏感。

五、保留短时运动、重叠和无全局校正限制

这篇论文的新意主要是系统选择、对应阈值设计和充分的组合检验。它没有重新发明 ICP，而是追问：哪些复杂性解决了真实瓶颈，哪些只是掩盖基础实现中的缺陷？这对阅读新方法很有帮助，因为公平基线应该先把基本输入与调度做好。

以下是根据机制的分析。恒速模型遇到一帧内的强非匀速运动会失准；大面积动态物体可能共同支持错误运动；长走廊和重复结构削弱唯一性；错误配准反过来更新阈值，还可能形成自我强化。自适应并不等于永不失稳。

少参数也不是零参数。量程、体素尺度、启动门限和终止条件都隐含工程取舍。统一参数在若干数据集有效，是实验证据；它不能覆盖所有雷达噪声、时间采样模式和极端运动。

六、追问：要检验自适应阈值的作用，为什么不能同时改变下采样和运动预测？

要检验自适应阈值的作用，为什么不能同时改变下采样和运动预测？

参考答案：两者都会改变对应分布和初始残差，无法再把差异单独归因于阈值。应固定预测、点集与地图管理，仅替换阈值规则并检查误差与失败案例。跨场景竞争力也仍依赖连续扫描重叠和短时运动可预测性。

七、带着传感器条件回看鲁棒配准与惯性融合

平行路线：TEASER：错误匹配占多数时，怎样配准点云并检查答案。全局鲁棒对应估计解决另一个初始化与外点任务。
平行路线：FAST-LIO2：直接用点云定位，为什么先要解决地图更新。加入惯性与迭代融合改变了可用信息，不能视为同预算替代。

八、自测与参考答案

1．为什么用已经去畸变的 KITTI 点云，无法验证自己的去畸变模块？

答：输入已包含上游补偿的收益，再次处理不能隔离本模块贡献，还可能重复补偿。需要原始扫描及正确逐点时间，在相同后端下比较有无补偿，才能判断因果作用。

2．自适应门限变大，是否意味着系统更鲁棒？

答：不一定。它可能挽回因初值误差而落在较远处的正确对应，也可能纳入更多错配。应该一起查看几何一致性、有效对应分布和最终误差，而不能把接受点数越多当作越好。

3．这篇论文是否证明复杂方法没有价值？

答：没有。它证明合理实现的简单基线在所测条件下很强，并揭示某些复杂组件的收益需要重新隔离验证。复杂方法仍可能解决恒速失效、稀少重叠或全局漂移等这里没有覆盖的问题；判断标准应是新增能力及其代价。