028｜ICP：不知道谁对应谁，也可以交替求解形状对齐

总览 · 目录 · 上一篇 · 下一篇

论文：Paul J. Besl、Neil D. McKay，A Method for Registration of 3-D Shapes，1992，IEEE TPAMI 14(2):239–256。原始期刊论文 · DOI。

补充来源：同版扫描转录。

先接住前作的问题

Horn解决已知对应的刚体配准，真实点云却常不知道谁对应谁。ICP把对应与变换交替求解；KISS-ICP继续用这副骨架，并把连续扫描的预测与鲁棒处理做扎实。

五个前置概念

五个前置概念：刚体变换保持形状内部距离；最近点查询提供当前几何对应；闭式最小二乘求固定对应的最佳对齐；交替优化轮流更新对应与位姿；局部极小表示附近改善困难但未必全局最好。

一、问题设定：未知点对应与刚体变换怎样交替求解

Horn 方法解决了已知点对应时的刚体配准。但扫描一个零件时，测得的点没有天然标签，无法知道“第一个扫描点”对应 CAD 模型的哪个位置。ICP 交替做两件事：把每个点配到当前最近的模型位置，再求使这些临时对应更接近的刚体变换。

背景：几何最近与物理对应不是同一个概念

输入是源数据点集 $P=\{p_i\}$、目标几何模型 $S$，以及初始变换。输出为 $R\in SO(3)$、$t\in\mathbb R^3$。没有奖励或策略训练；目标是最小化形状之间的几何距离。

原文的主要应用是把未精确固定的刚体扫描数据对齐到理想模型，以进行形状检验。引言将统计离群点的去除留给预处理，并未处理不同点具有明显不同不确定性的情形。这与后来开放环境中的激光里程计有重要区别：动态车辆、非重叠区域和距离相关噪声都可能违背早期简化。

模型可以比数据完整，例如扫描只覆盖 CAD 表面的一部分。但源数据中如果有大量不属于目标的点，原始单向距离会把它们也强行拉向模型。原文第 V-C 节明确区分了这两种“部分匹配”，因此“支持部分形状”不能泛化成“对任意低重叠点云都鲁棒”。

二、交替找最近点与刚体拟合，逐步证明目标不增

方法：一个带隐含对应的优化问题

目标可写成

$$F(R,t)=\frac1N\sum_i d(Rp_i+t,S)^2,$$

其中 $d(y,S)=\min_{q\in S}\|y-q\|$。距离中的最近点随变换改变，让整体问题非凸。引入临时对应 $q_i$ 后，可以拆开。

第 $k$ 轮先计算

$$q_i^{(k)}=\arg\min_{q\in S}\|R_kp_i+t_k-q\|,$$

再固定这些对应，求

$$(R_{k+1},t_{k+1})=\arg\min_{R\in SO(3),t}\sum_i\|Rp_i+t-q_i^{(k)}\|^2.$$

最近点查询通常占主要计算成本。对目标点集朴素搜索为每源点检查全部目标点；对曲面则需要求几何实体上的最近点。加速索引可以降低常见查询成本，但不改变 ICP 的局部优化本质。

为什么“两个步骤都变好”能推出同一目标下降

源点记为 $p_i$、目标点为 $q_j$，$T$为刚体变换，$c_i$为源点 $i$对应的目标索引；记平方误差 $E(T,c)=\sum_i\|Tp_i-q_{c_i}\|^2$。最近点步骤在当前 $T^k$ 下选 $c^{k+1}$，故 $E(T^k,c^{k+1})\le E(T^k,c^k)$；固定对应后精确求刚体最小二乘，得到 $E(T^{k+1},c^{k+1})\le E(T^k,c^{k+1})$。串起两个不等式，才得到单调性。它依赖两个子步针对同一目标；改变裁剪集合、鲁棒权重或近似最近邻后，不能不检查条件就沿用原证明。

一维教学演算。 源点为0、1，目标点为0、1、3，初始平移1.4。变换后的1.4、2.4分别对应1、3，平移最小二乘给 $t=[(1-0)+(3-1)]/2=1.5$，平方误差从0.52降到0.5；下一轮对应不变，平移仍为1.5。但平移0可使源点准确落在目标0、1上，误差为0。这个例子没有等距对应的平局：局部下降与稳定对应均成立，仍可停在非全局最优答案。这是离散对应造成的盆地问题，不是刚体闭式求解不准确。

Horn可以作为固定对应配准的模块组合；TEASER是已给候选匹配、含大量外点的另一问题设置；KISS-ICP直接继承ICP骨架，但依靠扫描间预测和工程设计缩小错误对应的风险。原文第243–245页给出算法与误差单调性证明，讨论的是局部距离极小收敛，不能被改写成全局配准保证。

单调下降到底证明了什么

令 $G(T,Q)$ 为固定对应下的平方误差，$Q_k$ 是 $T_k$ 下的精确最近点。则

$$F(T_{k+1})\le G(T_{k+1},Q_k)\le G(T_k,Q_k)=F(T_k).$$

右边的不等式来自刚体最小二乘，左边来自重新选最近点。误差非负，所以误差序列单调有下界。这是第 IV 节证明的核心，也是可以独立手算验证的机制。

但这个链条并没有证明 $T_{k+1}$ 越来越接近真实位姿。一个错误形状匹配也可以有很小距离；对称物体更可能存在多个等价解。即使误差收敛，也不应自动推断所有变换参数都有唯一正确极限。

近似最近邻、随机下采样、不同的距离指标和拒绝规则，会改变上述证明的条件。工程版本仍可能更好，但不能不检查假设就照搬原始单调性结论。

两个交替步骤各自不增同一目标，才能接成单调性论证。下面用周期栏杆构造一个残差很小的错误位姿，提醒实现必须把初始化与物理正确性另行检验。

三、实现配准循环，用周期结构检查错误稳定解

第二步就是前面 Horn 一章的刚体最小二乘：去中心化、形成互协方差、由四元数特征值问题求旋转，再恢复平移。原文采用四元数方案；下面的伪代码保持这个版本，不把现代 point-to-plane 或鲁棒核混入原始算法。

输入：固定源点集 P，目标模型 S，初始刚体变换 T
循环：
    用 T 变换源点，并求每个变换点在 S 上的最近点 Qi
    固定 Qi，求原始 P 到 Qi 的最佳刚体变换 Tnew
    计算平方距离误差和误差下降量
    更新 T = Tnew
    若误差变化低于阈值或达到迭代限额，停止
输出：变换、残差和是否满足停止条件

这里每轮从固定原始点集直接求总变换。另一种实现对当前点集求增量，再左乘累计变换；两种写法都可行，但不能求了总变换后又当增量重复累乘。

自构例子：错误对应可以形成稳定答案

想象目标模型由周期相同的栏杆组成，源点只看到其中几根。把源点错移一个栏杆间距后，所有点仍可以找到很近的邻居。ICP 的每一步都可能合法下降，最后却给出错了一个周期的平移。

加入一块只有某根栏杆附近存在的独特标志，或提供粗略里程计，才可能打破这个歧义。这个例子说明初始化和可辨识性是不同问题：前者决定进入哪个吸引区域，后者决定数据是否足以区分候选解。

四、读八点示例与初始化反例，区分残差和正确性

读图：上下对照配准前后的两个小点集，观察形状通过刚体变换重合。这里的收敛展示依赖该初值和几何，不能从这一对图推出任意部分重叠点云都收敛到正确解。

收敛曲线说明给定对应更新规则下的目标下降；物理配准正确还要检查初始位姿、重叠范围及几何退化。

读图：先确认两组点的规模不同，再对照图4与图5；表中坐标是可复核的输入，不能将配准成功解释为预先已知点对对应。

表 I、图 4–5，第 VI-A 节给出配准前后的小点集。六次迭代、不到一秒、RMS 0.437608 mm（约0.438 mm） 是这个示例的条件性结果。它展示无需点顺序和预先对应；不意味着任何八对十一点问题都只需六次。

本页定位：图 4内嵌图 · 图 5内嵌图 · 表 I内嵌图。

图 6–7，第 VI-B 节展示部分带噪空间曲线的对齐，说明最近点算子可以作用于曲线模型，而不局限于离散点。具体矩阵受转录质量影响，本文不复抄；曲线示例也不能证明所有曲线都有唯一对齐。

读图：两条曲线的起始相对位姿差异很大，而且其中一条只覆盖局部；这同时考验初值搜索、局部对应和噪声稳健性。

读图：与图6并排比较重叠段；未重叠部分仍存在，因此视觉贴合只说明该例的局部匹配效果。

图 8 及第 VI-C 的 Bézier 曲面实验用带噪采样点展示自由曲面配准，支持表示通用性。最近点仍需针对这种曲面正确计算，通用框架不等于任意几何模型的查询都同样便宜。

读图：散点和网格是不同表示，最近点需落在曲面上；这是ICP可处理点到曲面匹配的例子，图8本身是配准前状态。

第 V-B 节反例构造带细尖刺的近球形物体：对于预先固定的一组初始旋转，可以设计使这些初值都无法找到正确对齐的形状。它直接限制了多初值策略的保证，远比“多试几次就全局最优”的口头描述准确。

五、检查重叠、采样密度、离群点与局部极值

第 V 节讨论用多初始旋转和平移扩大覆盖。这个策略增加计算预算，其有效性与物体复杂性有关，不能用某个固定数量给所有物体保证。原文并没有忽略局部极小，反而花了专门篇幅解释它。

以下为应用分析。一个长走廊沿轴方向平移后，许多局部平面距离变化很小；一个圆柱绕自身轴旋转后形状近乎不变。这些几何退化让残差对某些位姿分量不敏感。低残差可能来自缺少约束，也可能来自正确对齐，因此还需检查几何信息量。

密集区域会在等权求和中占据更多影响，采样分布因此改变目标。对有噪声的两片同一表面，最近点也不是无偏的真实对应。鲁棒核、法向距离、采样平衡和外部运动先验后来各自处理这些问题，但它们改变了原始方法，评估时应拆清。

六、追问：改用截断对应或近似最近邻后，为什么不能直接引用原单调下降证明？

改用截断对应或近似最近邻后，为什么不能直接引用原单调下降证明？

参考答案：原证明比较的是同一目标下的对应更新与变换更新，并依赖相应最小化关系。改变对应集合、距离或查询精度后，这两个不等式需要重新检查。实现可以因此更实用，但应记录实际目标与接受准则，而不是把原证明自动移植到修改版本。

七、把交替配准接入连续扫描

• 直接继承：KISS-ICP：把简单基线做好，究竟能走多远。在连续扫描条件下加强预测、阈值和鲁棒配准，仍以ICP循环为骨架。

八、自测与参考答案

问题1：ICP 每步误差不增，为什么仍可能得到错误位姿？

答：误差衡量的是点到当前最近模型位置的距离，而非真实对应误差。重复或对称几何可以产生错误但距离很小的解。单调下降只约束目标数值，不提供全局正确性。

问题2：模型比扫描完整，与扫描包含很多非模型点，有什么不同？

答：单向距离要求每个源点都能在目标上找到解释，允许目标存在额外区域。若源点包含背景或其他物体，它们也被强行纳入平方误差，可能拉偏变换。因此两种“部分重叠”不能一概而论。

问题3：先求 Horn，再重新匹配一次，为什么通常还不够？

答：第一次 Horn 只对当前对应最优，对齐后最近点关系可能改变。重新匹配会产生新的、通常更合理的拟合目标，还需再次求刚体变换。只有当对应与变换基本一致时，交替循环才趋于稳定。