40 3D 匹配

ICP（章节 39）解决的是”把两团已经大致对齐的点云压到亚像素吻合”——它是一台精化机器，却天生近视：初值偏过几十度，它就锁进错误的局部极小，再也爬不出来。可产线上的相机看到的从来不是一团孤立的点云，而是一框杂乱场景：地面、料框壁、相邻工件、噪声，待找的零件藏在其中，位姿完全未知。3D 匹配（3D matching） 负责的正是这”从无到有”的一步——在杂乱场景点云里定位已知模型，给出它的粗略 6 自由度（6 degrees of freedom, 6DoF）位姿（pose），再把这个粗位姿交给 ICP 精化。它是 2D 模板匹配（template matching）（章节 16）与形状匹配（shape matching）（章节 17）在三维世界里的对应物，也是机器人抓取与装配的眼睛：先知道零件”在哪、朝哪”,机械臂才谈得上去抓。

本章的模型是一个非对称 L 形支架，约 5400 个表面点：长臂、短臂构成 L，长臂顶端再加一块凸台（图 40.1 (a)）——凸台刻意打破了 L 本身残留的对称性，让主轴方向唯一可辨，原因见小节 40.2。场景（图 40.1 (b)）含两个该支架的实例，各取不同 6DoF 位姿：实例 1 洁净完整，实例 2 被遮挡（去掉约 1/3 表面）；此外有一块地面、一个干扰圆柱，全场叠加 \(\sigma=0.1\,\text{mm}\) 的高斯噪声。任务是把两个实例都找出来、定准位姿，同时不把圆柱误当成支架。

40.1 6 自由度位姿搜索

2D 匹配搜索的是三个参数 \((x, y, \theta)\)——平面平移加面内旋转；上一两章里模板/形状匹配能在角度 × 尺度网格上逐格暴力评分，靠的就是搜索空间只有三维、且图像金字塔把它逐层缩小。换到三维，刚体位姿有 6 个自由度：三个平移 \((t_x, t_y, t_z)\) 加三个旋转（绕三轴）。维度从 3 升到 6，搜索空间不是翻倍而是指数爆炸——若每个旋转轴离散成 \(K\) 档、每个平移轴 \(K\) 档，候选总数是 \(K^6\)。章节 16 里”暴力评分扛不住、必须金字塔”的动机，在 3D 里被放大到无法回避的地步：哪怕每轴粗到 \(30^\circ\)/\(10\,\text{mm}\)，\(K\!\approx\!12\) 也意味着上百万候选，每个候选还要在数千点上做最近邻评分。

6DoF 之”6”是刚体在三维空间的全部自由度：平移 3 + 旋转 3。若零件还可能缩放（物距未标定），再加 1 个尺度自由度——但工业 3D 相机已标定到毫米，尺度通常固定，这是 3D 匹配比 2D 少操心的一点。

结论和 2D 一样、却更迫切：不能暴力，必须 coarse-to-fine。先用廉价手段在场景里圈出”可能是零件”的少数候选、各给一个粗位姿（粗匹配 coarse matching），再只对这几个候选跑 ICP 把位姿压到噪声底（精匹配 fine registration）。粗匹配负责把 \(K^6\) 的搜索塌缩成几个假设，精匹配负责把每个假设的几度几毫米误差消干净——两者缺一不可，这条流水线是本章的主线。

40.2 粗匹配方法

粗匹配的第一步不是匹配，而是场景分割：把一框点云切成”一个个候选物体”。沿用点云预处理（章节 38）的两板斧——先 RANSAC 拟合最大平面、把地面剔除（地面是场景里点数最多的平面，随机采三点拟合、统计内点，200 次迭代取最优）；地面一去，悬在其上的物体彼此分离，再做欧氏聚类（Euclidean clustering）：把空间体素化，相邻体素内距离小于阈值（本章 \(4\,\text{mm}\)）的点用并查集合并，每个连通块就是一个簇。本章场景去掉地面后干净地分出三簇：两个支架实例 + 一个圆柱。

每个簇都要求一个粗位姿。最朴素的办法是 主成分分析（principal component analysis, PCA）：对簇点求质心作为平移初值，求协方差矩阵的三个特征向量作为物体的三条主轴，与模型自身的主轴对齐，就得到旋转初值。原理直白——一个细长的 L 支架，点云的最大方差方向必沿长臂，PCA 能一眼”看出”这条轴。但 PCA 有两处先天局限：其一，主轴有符号与排列的歧义——特征向量正负任意、近似相等的特征值下排列也可摆动，三轴一共 \(24\) 种右手系组合都”同样合理”，必须逐一试、按”哪个让模型最贴合场景”来消歧；其二，对遮挡敏感——删掉零件一部分，点云的方差分布就变，主轴随之偏斜，PCA 给出的旋转可能错出几十上百度。后者正是实例 2 的故事（小节 40.3）。

PCA 之外另有一类更强的粗匹配：点对特征（point-pair feature, PPF）——对模型上成对的点，用其距离与两点法向夹角构成 4 维特征、投票出位姿，对遮挡与杂乱更稳健，是 Drost 等工业表面匹配的基石。本章用 PCA + 多候选搜索演示原理；PPF 实现量更大，留作延伸。

本章对每个簇生成全部 \(24\) 个 PCA 候选位姿，各跑一遍宽捕获半径的快速 ICP，按场景覆盖度（下节定义）择优——用一次廉价的”试错搜索”补上 PCA 单一估计的脆弱。

40.3 粗→精流水线

这是本章的核心教学实验：粗匹配喂初值、ICP 压噪底，两个实例把这条流水线的两种典型剧情演全了。

实例 1（洁净）：表面完整、点云稠密，PCA 主轴几乎不偏，胜出候选的粗位姿已经是 旋转误差 \(0.02^\circ\)、平移误差 \(0.01\,\text{mm}\)——满采样下 PCA 几乎一步到位。ICP 接手后把它精化到 \(0.01^\circ\)/\(0.00\,\text{mm}\)、内点残差 \(0.162\,\text{mm}\)，场景覆盖与模型覆盖都达 \(100\%\)。这里 ICP 几乎无事可做，因为粗位姿已落在噪声底附近。

实例 2（遮挡）：去掉约 1/3 表面后，主轴严重偏斜，\(24\) 候选里最好的那个粗位姿竟偏到 旋转 \(100.5^\circ\)、平移 \(21.8\,\text{mm}\)——单看这个数，PCA 几乎是失败的。但它仍把模型摆进了正确的簇、正确的大致位置，足够 ICP 在宽捕获半径下逐步收紧最近邻、一点点把模型”拧”回去。最终 ICP 收敛到 \(0.38^\circ\)/\(0.90\,\text{mm}\)、残差 \(0.400\,\text{mm}\)。两阶段的缩减极为可观：旋转 \(100.5^\circ\!\to\!0.38^\circ\)（约 \(\times 266\)）、平移 \(21.8\!\to\!0.9\,\text{mm}\)（约 \(\times 24\)）。

洁净实例的残差 \(0.162\,\text{mm}\) 恰落在噪声底附近：各向独立 \(\sigma=0.1\,\text{mm}\) 的点，到真实表面的 RMS 约 \(\sigma\sqrt{3}\approx 0.17\,\text{mm}\)。ICP 收敛到这个量级就到头了——再小就是过拟合噪声。遮挡实例的 \(0.40\,\text{mm}\) 略高，是部分数据下的极限，非算法缺陷。

图 40.2 把两条 ICP 收敛轨迹画在一起（纵轴残差取 \(\log\)）。橙线是遮挡实例：从粗位姿对应的约 \(4\,\text{mm}\) 残差出发，随迭代逐级下降到约 \(0.4\,\text{mm}\)——每一台阶是捕获半径收紧后纳入更多正确对应、甩掉错误对应的结果。蓝线是洁净实例：起点就压在底部、几乎水平——它一开始就在噪声底，ICP 只是确认而非搬运。这张图是章节 39 “ICP 需要好初值”论断的闭环注脚：好初值（洁净）让 ICP 无事可做，差初值（遮挡）让 ICP 大显身手，而差到没边的初值会让 ICP 直接失败——本章靠”多候选 + 宽半径”把实例 2 的初值救到 ICP 够得着的范围。

图 40.2: ICP 收敛轨迹（横轴迭代次数，纵轴内点残差 \(\log\) 刻度，网格线 \(0.1/1/10\,\text{mm}\)）。橙=遮挡实例，从约 \(4\,\text{mm}\) 逐级降到约 \(0.4\,\text{mm}\)；蓝=洁净实例，始终贴在噪声底（约 \(0.16\,\text{mm}\)）几乎水平。

40.4 遮挡与覆盖度

章节 17 里 2D 遮挡让分数按可见比例退化；3D 匹配要把”覆盖”拆成两种方向相反的覆盖度（coverage），它们度量的是不同的东西。

场景覆盖度（scene coverage）：把模型摆到候选位姿后，簇里有多少点落在模型表面附近。它问的是”我找到的这堆点，有多少能被模型解释”。遮挡只删场景点，剩下的点照样贴在模型上，所以场景覆盖度对遮挡不敏感——正因如此，它被选作接受判据：覆盖度高就接受这个匹配。
模型覆盖度（model coverage）：反过来，模型有多少点找到了对应的场景点。零件被遮住的部分没有场景点支撑，这部分模型点”无人认领”，所以模型覆盖度随遮挡直接下降——它正好用来度量遮挡严重程度。

实验把两者的分工演得很清楚。遮挡删去约 1/3 表面后：模型覆盖度从洁净的 \(100\%\) 掉到 \(46.5\%\)（近乎减半，如实反映了遮挡量），而场景覆盖度只从 \(100\%\) 降到 \(79.3\%\)（仍远高于接受阈值）。图 40.3 里蓝色的遮挡实例肉眼可见缺了一段几何，但它剩下的点仍严丝合缝贴在模型上——这正是”场景覆盖度高、模型覆盖度低”的图像。若用模型覆盖度当接受判据，遮挡实例会被冤枉拒绝；用场景覆盖度，它顺利通过。

干扰圆柱是反例：它的几何与 L 支架毫不相干，无论怎么摆模型，簇点都贴不上模型表面——场景覆盖度仅约 \(5.4\%\)，远低于接受阈值，被干净剔除。覆盖度判据既要接住遮挡的真零件，又要挡住外形不符的假目标，圆柱正是后者的试金石。

图 40.3: 场景分割结果：灰=地面（已由 RANSAC 标出），红/蓝/绿三簇为欧氏聚类输出——红为完整 L、蓝为遮挡 L（明显缺一段长臂几何）、绿为干扰圆柱。

图 40.4 是最终匹配：模型按两个恢复位姿叠回场景，洁净实例处红色、遮挡实例处品红，轮廓都严丝合缝；圆柱因覆盖度不足未被叠加。遮挡实例能定位成功，是”PCA 单独不够、朴素 ICP 单独也不够、二者接力才够”的合力——这与章节 17 中 2D 形状匹配”分数按可见比例退化、minScore 即可容忍遮挡比例”的逻辑一脉相承，只是 3D 把”一个分数”拆成了”两种覆盖度”。

图 40.4: 匹配结果：模型按两个恢复的 6DoF 位姿叠回场景灰底，洁净实例红色、遮挡实例品红，均吻合；干扰圆柱覆盖度不足，未被叠加。

40.5 SciVision 实现

按惯例先如实记录 SDK 实测。SciVision 备有两套 3D 匹配接口：SciSv3DSurfaceMatch（CreateSurface3DModel/FindSurface3DImageModel，距离图表面匹配）与 SciSv3DPointsMatching（GetTransformationICP/GetTransformationPCA，点云配准）。本章用子进程探针逐一调用，两者均惰性失效：返回码 rc=0（不报错、不崩溃），但表面匹配找到 0 个匹配、点云 ICP/PCA 把已知平移恢复成 \((0,0,0)\)，DLL 还向 stderr 打出一句 "Directory does not exist."——与本书 Part IX 多个 3D 模块同款签名（疑似缺运行时资源目录）。SDK 不可用，本章内容由手写流水线承担。

手写流水线分四步，关键片段如下。

// 1) RANSAC 去地面：200 次随机三点拟合平面，取内点最多者
std::vector<char> ground = ransacPlane(scene, /*tol*/ 0.4);

// 2) 欧氏聚类：体素 + 并查集，tol=4mm，丢弃 <200 点的小簇
std::vector<std::vector<int>> clusters = cluster(rest, 4.0, 200);

// 3) 每簇：PCA 求质心 + 主轴，24 个签名置换候选各做宽半径快速 ICP，
//    按场景覆盖度择优（消歧 + 救回偏斜主轴）
pca(clu, cs, Vs);                       // 簇质心 cs、主轴 Vs
for (auto& P : perms /*24 个右手系置换*/) {
    matMul(Vs, P.data(), VsP); matMul(VsP, VmT, Rc);   // 候选旋转
    icp(modelScreen, clu, Rt, tt, 25, 14.0, 2.5);      // 宽捕获半径筛选
    double sc = sceneCov(model, Rt, tt, cluDown, 0.6, rms);
    if (sc > bestSC) { /* 记下最优候选 */ }
}

// 4) 以最优候选为初值做完整 ICP（Kabsch 闭式解，3×3 SVD 由 Jacobi 实现）
best.resid = icp(modelDown, clu, R, t, /*iters*/ 60, /*d0*/ 10.0, /*d1*/ 0.8);
best.sceneScore = sceneCov(model, best.R, best.t, cluDown, 0.5, rmsS);
best.ok = (best.sceneScore > 60.0 && best.resid < 1.5);   // 覆盖度 + 残差双判据

每次 ICP 迭代用 Kabsch 求最优刚体变换：对配对点去质心、累加协方差矩阵 \(C\)，对 \(C^{\mathsf T}C\) 做 Jacobi 特征分解实现 \(3\times 3\) 的 SVD，由 \(R=UV^{\mathsf T}\) 得旋转（并校正反射，保证 \(\det R=+1\)），平移由两质心之差求出。捕获半径从 \(d_0\) 线性收紧到 \(d_1\)，正是图 40.2 橙线逐级下降的来由。整套数学完全自包含——这呼应章节 37 与本书的一贯观察：点云类算法（ICP/Kabsch/PCA/PPF/kd 树）多需自研或依赖专用库，SDK 在 3D 匹配上常只堪作 IO 与交叉验证。工业 3D 匹配走到深处，自研几乎是常态。

40.6 工业案例

工业案例：散乱堆叠零件的拆垛抓取

料框里同种零件随机堆叠（bin picking），机械臂要逐个找位姿、依次抓走。3D 相机扫出一框点云，流程与本章一致：去框底平面、欧氏聚类切出一个个候选，每个簇上求 PCA 粗位姿 + ICP 精化。难点在于堆叠互遮——上层零件压住下层，多数实例的模型覆盖度很低，若按模型覆盖度接受就会大面积漏抓。对策正是本章的判据拆分：按场景覆盖度接受（抗遮挡），再按可抓面可见性排序，优先抓取最完整、夹爪能稳稳贴合的那个。框壁与相邻零件的干扰，靠覆盖度阈值挡掉——外形不符的点贴不上模型，与本章圆柱被 \(5.4\%\) 覆盖度剔除同理。教训凝成一句：3D 匹配的鲁棒性 = 分割质量 × 覆盖度判据，遮挡之下的生产法则是”先抓看得最全的”。

40.7 小结

3D 匹配是 ICP 的前置：ICP（章节 39）只精化、需好初值；3D 匹配在杂乱场景里定位已知模型、给出粗 6DoF 位姿喂给 ICP。它是 2D 模板/形状匹配（章节 16、章节 17）的三维对应。
6 自由度令暴力搜索爆炸（\(K^6\)），必须 coarse-to-fine：粗匹配（RANSAC 去地面 + 欧氏聚类 + PCA 主轴/质心，\(24\) 候选消歧）把搜索塌缩成几个假设，ICP 把每个假设压到噪声底。
粗→精接力实测：洁净实例 PCA 已近精（\(0.02^\circ\)/\(0.01\,\text{mm}\)），ICP 收到 \(0.01^\circ\)/\(0.00\,\text{mm}\)；遮挡实例粗位姿偏 \(100.5^\circ\)，靠宽半径 ICP 救回到 \(0.38^\circ\)/\(0.90\,\text{mm}\)（旋转 \(\times 266\)、平移 \(\times 24\)）。PCA 单独与朴素 ICP 单独都不够，接力才够。
两种覆盖度分工：场景覆盖度（找到的点占比，抗遮挡）作接受判据，模型覆盖度（模型被解释占比）作遮挡严重度。遮挡使模型覆盖 \(100\%\!\to\!46.5\%\)、场景覆盖仅 \(100\%\!\to\!79.3\%\)；干扰圆柱按 \(5.4\%\) 场景覆盖被剔除。
SDK 如实：SciSv3DSurfaceMatch 与 SciSv3DPointsMatching 均惰性失效（rc=0 零匹配/恢复零位姿），本章由手写流水线（RANSAC + 聚类 + PCA + Kabsch ICP）承担——工业 3D 匹配常需自研或专用库。

3D 匹配的原理与点对特征、表面匹配的工程实践，在 Steger 等人的著作中有系统论述 (Steger, Ulrich, 和 Wiedemann 2018)。把局部几何编码成描述子再投票求位姿，是这一族方法的共同思路：Drost 等人用点对特征（PPF）做全局建模、局部匹配，是工业表面匹配的代表 (Drost 等 2010)；更早的 Johnson 与 Hebert 以自旋图像（spin image）在杂乱场景中识别三维物体 (Johnson 和 Hebert 1999)；Rusu 等人提出的快速点特征直方图（FPFH）则是点云粗配准里最常用的描述子之一 (Rusu, Blodow, 和 Beetz 2009)，可为本章 PCA 之外提供更稳健的粗位姿来源。