39  ICP 配准与 3D 校正

把同一个工件从两个视角各扫一次，得到两片点云，要拼成完整的表面，先得把它们对齐；把扫描到的实物与设计的 CAD 模型比对、判断装配是否到位，先得把实物云”摆”到模型坐标系里；机器人要去料框里抓一个零件，先得知道这个零件相对夹爪的精确位姿——三件事看似无关，骨架却是同一个：把一片点云刚体变换到另一片上，使两者尽可能重合。这就是三维配准（registration），而 ICP（iterative closest point，迭代最近点）是工业现场承担配准的主力算法。

本章用一个合成场景贯穿全部实验（图 39.1）：一片模型云（model）——输送带平面上放着一个方盒和一个球冠，共 5678 点；一片扫描云（scan）——把模型施加一个已知的刚体变换（绕轴 \((1,2,2)\) 旋转 \(12°\)、平移 \((5,-3,2)\,\text{mm}\)）、叠加 \(\sigma=0.1\,\text{mm}\) 的测量噪声，再裁掉一侧、并在右侧补上一道模型里根本不存在的”侵入墙”得到，共 6497 点。两片云只有 4310 点真正对应，另有 2187 点（33.7%）是无对应的非重叠部分。图中模型为蓝、扫描为红，初始明显错位，右侧那道孤立的红色竖墙就是侵入结构——它在模型里没有任何对应点，却必须被算法识别并排除。这个”非完全重叠”的设定不是为难自己：真实扫描里，相机视野边界、相邻零件、料框壁带来的非重叠几乎是常态。

图 39.1: 模型云（蓝）与扫描云（红）的初始位姿，XZ 正交侧视。两片云明显错位；右侧孤立的红色竖墙是只存在于扫描中的侵入结构（料框壁的抽象），在模型里没有对应点。

39.1 配准问题与 ICP

刚体配准要求解的，是一个旋转矩阵 \(\mathbf R\)（\(3\times 3\)，正交且 \(\det\mathbf R=1\)）和一个平移向量 \(\mathbf t\)，使得把源云每个点 \(\mathbf p_i\) 变换后与目标云重合：\(\mathbf R\mathbf p_i+\mathbf t \approx \mathbf q_i\)。这与 章节 19 里二维位置补正求平面位姿 \((x,y,\theta)\) 是同一个问题的三维升级版——二维有 3 个自由度，三维刚体变换有 6 个（3 个旋转 + 3 个平移）。

难点在于：我们并不知道源云的哪个点对应目标云的哪个点。对应关系（correspondence）和变换（transform）互为因果——知道了对应就能解变换，知道了变换才能找对应。ICP 用一个朴素而有效的迭代打破这个鸡生蛋的死结，每一轮做三件事：

1. 最近邻找对应：把源云用当前估计的 \((\mathbf R,\mathbf t)\) 变换后，为每个源点在目标云里找最近邻对应（nearest-neighbor correspondence）。这一步是 ICP 的计算瓶颈，必须用 kd 树（kd-tree）把暴力 \(O(NM)\) 的最近邻查询降到 \(O(N\log M)\)（章节 37）。
2. 剔除远对应：最近邻并不保证是”真”对应——侵入墙上的点也会被指派一个最近的模型点，但这个对应是假的。按对应距离剔除离群对，是 ICP 处理非重叠的关键闸门。本章用中位数法：取所有对应距离的中位数 \(\tilde d\)，保留距离 \(\le 2.5\,\tilde d\) 的对应，把侵入墙这类大距离对应挡在门外。
3. 求最优刚体变换：在筛过的对应上，求使对应点对距离平方和最小的刚体变换。这一步有闭式解，即 Kabsch / Umeyama 算法。

第三步的数学值得展开，它是 章节 2 中 SVD 的一个漂亮应用。设筛后有 \(n\) 对对应 \(\{(\mathbf p_i,\mathbf q_i)\}\)，要最小化

\[ E(\mathbf R,\mathbf t)=\sum_{i=1}^{n}\bigl\|\mathbf R\mathbf p_i+\mathbf t-\mathbf q_i\bigr\|^2 . \]

先对两组点各自去质心：\(\bar{\mathbf p}=\frac1n\sum_i\mathbf p_i\)、\(\bar{\mathbf q}=\frac1n\sum_i\mathbf q_i\)，令 \(\mathbf p_i'=\mathbf p_i-\bar{\mathbf p}\)、\(\mathbf q_i'=\mathbf q_i-\bar{\mathbf q}\)。可以证明最优平移把两个质心对齐，旋转则只取决于去心后的点。构造 \(3\times3\) 的互协方差矩阵（cross-covariance）

\[ \mathbf H=\sum_{i=1}^{n}\mathbf p_i'\,\mathbf q_i'^{\,\top}, \]

对它做奇异值分解 \(\mathbf H=\mathbf U\boldsymbol\Sigma\mathbf V^\top\)，则最优旋转与平移为

\[ \mathbf R=\mathbf V\,\mathrm{diag}(1,1,d)\,\mathbf U^\top,\quad d=\operatorname{sign}\!\bigl(\det(\mathbf V\mathbf U^\top)\bigr),\qquad \mathbf t=\bar{\mathbf q}-\mathbf R\,\bar{\mathbf p}. \]

中间那个 \(\mathrm{diag}(1,1,d)\) 是关键的一笔：纯 \(\mathbf V\mathbf U^\top\) 可能得到一个行列式为 \(-1\) 的”镜像”（反射而非旋转），用 \(d\) 翻转最后一列即可强制其落回合法的旋转群 \(SO(3)\)。求出 \((\mathbf R,\mathbf t)\) 后累乘进当前估计，进入下一轮迭代，直到收敛。

为什么是去质心 + SVD？把 \(E\) 展开，平移项与旋转项在去心后解耦：平移的最优解必然让变换后的源质心落到目标质心上，剩下的旋转项 \(\sum_i\|\mathbf R\mathbf p_i'-\mathbf q_i'\|^2\) 等价于最大化 \(\operatorname{tr}(\mathbf R^\top\mathbf H)\)，而正交约束下这个迹由 \(\mathbf H\) 的 SVD 一步给出最优。这正是 章节 2 里”SVD 给出最优正交逼近”的直接落地。

39.2 收敛与精度

给 ICP 一个好初值——这里直接取单位阵（合成时旋转只有 \(12°\)、平移几个毫米，单位阵已落在收敛盆地内），跑点到点 ICP。结果：58 次迭代收敛，最终对应 RMS 0.6443 mm，恢复出的旋转角 \(10.69°\)（GT 为 \(12°\)，旋转角误差 \(1.97°\)），平移误差 \(0.233\,\text{mm}\)。图 39.2 显示对齐后红色扫描云稳稳贴合蓝色模型，而右侧那道侵入墙依然孤零零地立在原处——对应剔除正确地把它判为非重叠，没让它污染变换估计。

图 39.2: 点到点 ICP 收敛后（好初值）。红色扫描云与蓝色模型云重合；右侧侵入墙因对应距离过大被剔除，正确地未参与对齐。

值得玩味的是这个 \(0.6443\,\text{mm}\)：它收敛正确（位姿对了），却明显停在 \(0.1\,\text{mm}\) 噪声底之上。原因藏在点云的几何里——这片云被大面积的输送带平面主导，而点到点度量在平面上有个先天软肋：源点沿目标表面的切向滑动几乎不改变它到最近目标点的距离，目标函数在切向上近乎平坦。于是每轮迭代只能挪动一点点，收敛又慢又容易停在浅极小。图 39.3 的红色曲线把这件事画得很直白：RMS 快速降下来后拖着一条长长的、几乎贴平的尾巴，迭代了 58 次也只是缓慢蠕动到平台，而非利落地砸到噪声底。

终止判据：ICP 通常以”相邻两轮 RMS 变化小于阈值”或”达到最大迭代次数”为停。本章实现取 RMS 变化 \(<10^{-5}\,\text{mm}\) 即停，并设 60 次的迭代上限兜底。判据太松会过早收手、精度不足；太紧则在噪声底附近空转——平面主导云上点到点的长尾，正是判据难调的典型场景。

图 39.3: 三条 RMS 收敛曲线：红=点到点（好初值），降到平台后长尾蠕动；绿=点到点（坏初值），高位徘徊在错误极小；蓝=点到面（好初值），最快砸到接近零的噪声底。

39.3 局部极小：ICP 的命门

ICP 只保证局部收敛——它会老老实实滚到离初值最近的那个谷底，但不保证那是全局最优的谷底。把初值故意做坏来验证这一点：在好初值基础上额外绕 \(Z\) 轴多转 \(40°\)，其余不变，再跑同一套点到点 ICP。结果触目惊心：60 次迭代用满，最终 RMS 2.0824 mm，旋转角误差 29.3°，平移误差 8.20 mm。图 39.4 里那片绿色的”对齐”结果整体偏离模型——ICP 收敛了，但收敛到了一个错误的局部极小（local minimum）。图 39.3 的绿色曲线全程在高位徘徊，再也下不来：最近邻对应从一开始就配错了人，错误的对应解出错误的变换，错误的变换又巩固错误的对应，越陷越深。

图 39.4: 坏初值（额外绕 Z 转 40°）下点到点 ICP 收敛到错误局部极小。绿色为收敛结果，整体偏离蓝色模型；这是”ICP 需要好初值”的直观证据。

这就引出 ICP 工程应用的第一条铁律：ICP 是精化器（refiner），不是搜索器（searcher）。它能把一个”大致对”的位姿打磨到亚毫米，却无力从一个”差很远”的位姿里搜出正确答案。每个局部极小都有自己的收敛盆地（convergence basin），初值落在哪个盆地，ICP 就滚向哪个谷底；只有落在全局最优的盆地里，才有正确结果。

因此工业三维定位几乎都采用 coarse-to-fine（粗到精）流水线：先用一个对初始位姿不敏感的粗配准（coarse registration）算法——基于特征描述子或点对特征（PPF）的 章节 40 ——求出一个粗略但落在正确盆地内的初值，再交给 ICP 做精化。粗配准负责”找对盆地”，ICP 负责”滚到底”，各司其职。

39.4 点到平面变体

小节 39.2 暴露的点到点软肋——切向滑动不被惩罚导致收敛慢——有一剂对症的药：点到平面（point-to-plane）变体。它不再最小化源点到目标点的直线距离，而是最小化源点到目标点所在切平面的距离。设目标点 \(\mathbf q_i\) 处的表面法向为 \(\mathbf n_i\)（对目标云每个点取 \(k\) 近邻做 PCA、取最小特征值对应的特征向量即得），则每轮求解

\[ \min_{\mathbf R,\mathbf t}\sum_{i}\Bigl(\bigl(\mathbf R\mathbf p_i+\mathbf t-\mathbf q_i\bigr)\cdot\mathbf n_i\Bigr)^2 . \]

只惩罚法向上的偏离、放过切向上的滑动——这正好松开了卡住点到点的那道枷锁。每轮把小角度旋转线性化为 \(\boldsymbol\alpha=(\alpha_x,\alpha_y,\alpha_z)\)，连同平移凑成 6 维未知量，解一个 \(6\times6\) 的法方程即可。

效果立竿见影。同样的好初值、同样的数据，点到平面变体只用 9 次迭代就收敛到 RMS 0.0991 mm，旋转角误差 \(0.016°\)、平移误差 \(0.005\,\text{mm}\)——直接砸到 \(0.1\,\text{mm}\) 的噪声底，而点到点要 58 次还停在 \(0.6443\,\text{mm}\)。图 39.3 的蓝色曲线陡直下落、几步到底，与红色的长尾形成鲜明对照。

直观理解这个 9 比 58 的差距：平面区域上，点到点像让源点在目标面上”滑冰”，每步只能压实一点点法向距离，切向上的自由白白浪费；点到平面则把切向自由度直接交还给优化器，每一步都用在刀刃（法向）上。代价是要算目标云法向、且对法向估计质量敏感。正因更快更准，工业 ICP 实现大多默认点到平面，点到点更多作为教学基准与法向不可靠时的退路。

39.5 3D 校正

配准的近亲是 3D 校正（3D correction）：把工件从它被随手摆放的姿态”摆正”到一个基准坐标系里。章节 5 把像素标定到物理坐标系，3D 校正则把测得的工件姿态对齐到设计基准——量平面度、查倾斜、做后续测量之前，常常先得把基准面摆平。

最常见的一种是平面校正：工件本应水平放置，实际却带了一点倾斜，需要把它旋正到 \(z=0\) 基准面。做法很直接——对工件表面点用最小二乘拟合基准面（least-squares plane fit）得到法向 \(\mathbf n\)，再求一个把 \(\mathbf n\) 旋转到 \(+Z\) 轴的旋转矩阵，施加到整片点云上即可。求”把单位向量 \(\mathbf n\) 转到 \(\mathbf z\)“的旋转有现成构造：转轴取 \(\mathbf n\times\mathbf z\)、转角取两者夹角，代入罗德里格斯公式。

实验造一片带 \(7°\) 复合倾斜（绕一根略斜的轴）的平面、叠加噪声。拟合法向后旋正，残余倾角从 \(6.9999°\) 降到 \(0.0000°\)——一次拟合加一次旋转就把工件摆得严丝合缝。图 39.5 用 YZ 侧视画出前后对比：红色的倾斜点带被旋正成贴合 \(z=0\) 参考线的绿色平带。

图 39.5: 3D 平面校正，YZ 正交侧视。红色为校正前的 7° 倾斜平面点带；绿色为拟合法向、旋正到 +Z 后的结果，残余倾角 0.0000°，贴合 z=0 参考线（灰）。

往深一层看，校正就是与”理想姿态”做配准：它要对齐的目标不是另一片实测点云，而是一个解析定义的基准（这里是 \(z=0\) 平面）。所以它不需要最近邻迭代——目标几何是闭式的，一步拟合即得变换。把这条思路推广，与 CAD 基准面、基准轴、基准孔的对齐，都是同一类”对齐到理想几何”的校正问题。

39.6 SciVision 实现

本章的几何与数学——kd 树、Kabsch（SVD）、点到点 / 点到面迭代、平面拟合与旋正——全部手写，这正是本章的核心；SciVision SDK 在此只承担点云 IO 与交叉验证的旁证角色。这个选择有实测依据：本机 SDK 的若干 3D 入口在配准场景下并不可靠——Sci3DKdtree::CreateKdTree 的部分重载返回错误码 121106105，Sci3DAxisTransform::Transform3DPointArray 直接以 0xC0000005 崩溃；唯有 SciSv3DSurfaceCorrection::ApplyPlaneCorrection 可用，本章用它对平面校正结果做旁证。示例工程把每个 SDK 探针放进独立子进程运行，以隔离潜在崩溃、不影响主流程出图。

最近邻 + 对应剔除（中位数阈值）的主循环：

for (size_t i = 0; i < src.size(); ++i) {
    cur[i] = mul(res.R, src[i]) + res.t;   // 用当前位姿变换源点
    nn[i]  = kd.nearest(cur[i], d2[i]);    // 手写 kd 树最近邻
}
std::sort(sorted.begin(), sorted.end());
double med    = sorted[sorted.size() / 2];           // 对应距离中位数
double thresh = std::max(med * 2.5, 0.5);            // 中位数法剔除阈值
// 仅保留 dist <= thresh 的对应进入 Kabsch —— 侵入墙的大距离对应被挡掉

筛后对应交给 Kabsch 求闭式刚体变换（去质心 + 互协方差 SVD + 行列式修正），与 小节 39.1 的公式逐行对应：

Mat3 H = /* Σ p_i' q_i'^T */;
symEig3(mul(transpose(H), H), w, V);                 // H^T H 特征分解 → 右奇异向量
// U 列 = H v_i / σ_i；σ_i = sqrt(λ_i)
Mat3 VUt = mul(V, transpose(U));
double d  = det(VUt) < 0 ? -1.0 : 1.0;               // 防反射，强制 det(R)=+1
R = mul(mul(V, Mat3{{1,0,0, 0,1,0, 0,0,d}}), transpose(U));
t = ct - mul(R, cs);

点到面变体把每个对应线性化进一个 \(6\times6\) 法方程，未知量是小角度旋转 \(\boldsymbol\alpha\) 与平移：

double r = dot(s - q, nrm);          // 点到目标切平面的有符号距离
Vec3   c = cross(s, nrm);            // 旋转部分的雅可比
double a[6] = {c.x, c.y, c.z, nrm.x, nrm.y, nrm.z};
for (int u = 0; u < 6; ++u) {        // 累加正规方程 A x = b
    for (int v = 0; v < 6; ++v) A[u][v] += a[u] * a[v];
    bb[u] += -r * a[u];
}

平面校正则用 fitPlane（最小二乘解 \(z=ax+by+c\)）求法向，再用 rotateVecToZ（转轴 \(\mathbf n\times\mathbf z\) + 罗德里格斯）旋正——两个手写函数，配 SciSv3DSurfaceCorrection 旁证。完整可运行工程见 code/icp_registration/。

工业案例：机器人抓取的点云配准

料框里散乱堆放着待抓取的零件，机器人靠 3D 相机扫出点云配准到 CAD 模型、求 6DoF 位姿后规划抓取。早期方案直接上纯 ICP，产线频频翻车——零件初始姿态杂乱，ICP 总是陷入错误的局部极小，位姿偏了，机械手抓偏甚至抓空。改造的关键是补上粗配准这一环：先用 3D 匹配（章节 40）的点对特征算出粗略位姿，再交给 ICP 精化到亚毫米，位姿这才稳定下来。另一个绕不开的坑是非重叠：料框壁、相邻零件都会进入视野，这些点在目标模型上没有任何对应，必须靠对应剔除（距离阈值）一一挡掉，否则它们会把位姿往外拽偏。教训：ICP 是精化器、不是搜索器，永远要有一个粗配准在前面喂初值。

39.7 小结

• 配准 = 求刚体变换 \((\mathbf R,\mathbf t)\) 使两片点云重合，是二维位置补正（章节 19）的三维升级；ICP 用”最近邻找对应 → 剔除远对应 → SVD 求最优变换 → 迭代”的循环，打破对应与变换互为因果的死结。
• Kabsch / Umeyama 给出闭式最优刚体变换：去质心、构造互协方差矩阵 \(\mathbf H\)、对其 SVD，\(\mathbf R=\mathbf V\,\mathrm{diag}(1,1,d)\,\mathbf U^\top\)，其中 \(d\) 用于防止退化成反射——这是 章节 2 中 SVD 的直接应用。
• ICP 只保证局部收敛，对初值高度敏感：好初值下 58 次迭代收敛、RMS 0.6443 mm；坏初值（多转 40°）则陷入错误局部极小，旋转误差 29.3°。工业上必须 coarse-to-fine——先用 3D 匹配（章节 40）粗配准给初值，再用 ICP 精化。
• 点到平面变体远快于点到点：只惩罚法向偏离、放过切向滑动，本章实验 9 次迭代即到 RMS 0.0991 mm（点到点 58 次仍停在 0.6443 mm），故工业 ICP 多采用点到平面。
• 3D 校正是与”理想姿态”的配准：拟合基准面 + 旋正到 \(+Z\)，把 7° 倾斜一步降到 0.0000°；非重叠（侵入墙、料框壁）必须靠对应剔除挡掉，是配准可靠落地的前提。

关于 ICP、点云配准与三维位姿估计更系统的论述，可进一步阅读 Steger 等人的著作 (Steger, Ulrich, 和 Wiedemann 2018)。ICP 算法本身有两条经典源流：Besl 与 McKay 给出基于最近点迭代的点到点表述 (Besl 和 McKay 1992)，Chen 与 Medioni 则在配准多幅距离图时提出收敛更快的点到平面误差度量 (Chen 和 Medioni 1992)，二者正对应本章实验里 58 次与 9 次迭代的差别；各类采样、配对与剔除变体的系统比较见 Rusinkiewicz 与 Levoy 的综述 (Rusinkiewicz 和 Levoy 2001)。本章每一轮闭式最优刚体变换由去质心后互协方差矩阵的 SVD 给出（Kabsch/Umeyama），其最早的严格推导见 Arun、Huang 与 Blostein 的论文 (Arun, Huang, 和 Blostein 1987)。

Arun, K. S., Thomas S. Huang, 和 Steven D. Blostein. 1987. 《Least-Squares Fitting of Two 3-D Point Sets》. IEEE Transactions on Pattern Analysis and Machine Intelligence PAMI-9 (5): 698–700.

Besl, Paul J., 和 Neil D. McKay. 1992. 《A Method for Registration of 3-D Shapes》. IEEE Transactions on Pattern Analysis and Machine Intelligence 14 (2): 239–56.

Chen, Yang, 和 Gérard Medioni. 1992. 《Object Modelling by Registration of Multiple Range Images》. Image and Vision Computing 10 (3): 145–55.

Rusinkiewicz, Szymon, 和 Marc Levoy. 2001. 《Efficient Variants of the ICP Algorithm》. 收入 Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling (3DIM), 145–52. Quebec City, Canada.

Steger, Carsten, Markus Ulrich, 和 Christian Wiedemann. 2018. Machine Vision Algorithms and Applications. 2 本. Wiley-VCH.