上一篇讨论了一个容易误判的问题：参与配准的点拟合得很好，不代表独立目标也准确。这次把问题向前推进一步：如果两份点云根本没有可靠的逐点对应关系，程序需要自己找对应点，会发生什么？

ICP 是三维视觉中常见的工具。把 source、target、距离阈值和初始矩阵交给它，通常很快就能得到一个 4×4 变换。但“程序停止”“点云重合”“RMSE 很小”并不是同一句话，更不能自动推出“物理位置正确”。

本文范围：三维刚体点云配准，面向计算机视觉、三维重建与医学影像软件开发。采用 Open3D 0.19.0 的 legacy API（o3d.pipelines.registration），完整实验在 Python 3.11 中运行；不混用 Tensor API。所有坐标按毫米解释，数据完全合成，不涉及患者数据或临床操作，也不提供实际设备精度承诺。资料核对日期：2026 年 10 月 15 日。

一、ICP 到底在做什么？

图 1：ICP 的基本循环与总变换更新方向（原创教学图；实验为合成数据）

ICP 的全称是 Iterative Closest Point，迭代最近点。它把两个问题交替处理：先假设当前位姿大致可信，寻找近邻；再暂时相信这些对应关系，估计一个更合适的变换。下一轮继续重新找近邻。

1.1 本文固定 source → target 的方向

源点 p：source 坐标系中的列向量 目标点 q：target 坐标系中的列向量 q ≈ R p + t T = [[R, t], [0, 1]] NumPy 的 Nx3 行数组写法： transformed = points @ R.T + t

矩阵中 R 是旋转，满足 RᵀR=I、det(R)=1；t 是平移。本文不估计尺度，不能把毫米和米之间的 1000 倍差异当作一个刚体位姿问题交给 ICP 自动解决。

1.2 一轮迭代可以拆成三步

第一步，用当前 T 把源点变换到目标坐标系。第二步，在目标点集中给每个变换后的源点找近邻，只保留距离足够小的对应。第三步，基于这些临时点对求增量 ΔT，并更新 T_next=ΔT×T_current。这个左乘顺序来自增量作用于已经变换后的点。

注意“临时”二字：近邻只是几何上接近的采样点，不保证是同一块物理表面，更不保证具有相同语义。已知标志点对应关系的刚体拟合，与 ICP 自动寻找近邻，是不同的求解任务。

标准点到点 ICP 的核心代价可写为 E(T)=Σ||T p_i−q_c(i)||²，其中 c(i) 是当前匹配关系。匹配固定时可以解刚体最小二乘；匹配随位姿变化时，整体问题就存在多个可能的吸引域。本文后面的对称实验还会说明：有些错误分支在纯几何代价上甚至同样最优。

二、点到点、点到平面与其他方法如何选择？

点到平面的常见代价是 E_plane(T)=Σ[n_jᵀ(Tp_i−q_j)]²。它衡量的是沿法向的偏差，不是完整的三维点间距离。Open3D 的 ICP 教程 展示了点到点和点到平面的基本用法；表中其他方法的选择需要结合数据条件，不应把某种方法视为通用的精度升级。

2.1 为什么平面很容易“看着贴合，实际上滑了”？

设目标是 z=0 的理想平面，法向 n=(0,0,1)。点到平面残差只读取 z 方向偏差。在忽略边界、纹理和额外约束的理想模型中，沿 x、y 的平移，以及绕 z 的旋转，不改变这一残差。这是目标函数缺少信息，不是简单增加迭代次数就能补出来的。

真实有限平面中的边界、离散采样和对应点筛选会影响结果，因此不能据此宣称任何平面点云都完全无法配准。这里的推导解释的是：当有效区域主要提供相同法向时，某些自由度可能被约束得很弱。应主动增加不同方向的几何结构或外部位姿信息。

2.2 法向参数必须跟采样尺度一起看

邻域太小，法向容易受噪声影响；邻域太大，棱边两侧会被混在一起。法向尺度不是一个离开数据单位就能复用的常数。纯平方的点到平面残差中，单个法向整体反号不会改变残差平方，但法向方向一致性仍可能影响其他匹配检查、特征或重建流程。

三、为什么 ICP 会收敛到错误位置？

3.1 初值进入了错误的吸引域

两份扫描相距很远、旋转差很大时，最近点可能来自错误结构。一旦错误对应关系自洽，后续最小二乘会把结果往那个分支继续推。停止条件只说明变化已经很小或迭代预算用完，并没有提供正确性证明。

局部最优和几何歧义还需要分开看：前者可能通过更好的搜索找到较低代价，后者可能存在多个同样低的代价。重复齿状结构、规则阵列或对称零件，都可能让纯几何信息难以区分物理身份。本文实验专门隔离第二类问题。

3.2 重叠率低，或把不该参与的结构放进来了

如果源点只覆盖目标的一小部分，未重叠点会去寻找“最近但并非对应”的目标点。背景、支撑架、运动物体、扫描伪影也可能比真正的目标更有机会被匹配。先裁剪共同区域、移除已知无效结构，往往比继续调迭代次数更有意义。

3.3 阈值、密度与模型假设不匹配

距离阈值太紧，正确位姿附近仍可能没有足够对应；太松，许多不相关结构也被接纳。采样密集区域在普通逐点求和中占更大权重，体素下采样可以减轻这种偏置，却也可能删掉小特征。

刚体 ICP 还假设物体形状没有改变。软组织形变、运动模糊、不同重建算法的系统偏差，不能单靠一个 R、t 同时解释。此时应先审视数据与模型，而不是把残差都归咎于优化器。

四、先读懂两个分数，再看实验结果

图 2：fitness、inlier_rmse 与零对应陷阱（原创教学图；实验为合成数据）

K = 距离阈值内的 source→target 最近邻对应集合 fitness = |K| / source 点数 inlier_rmse = sqrt( Σ_(i,j)∈K ||T p_i - q_j||² / |K| )

这里限定 Open3D 0.19.0 legacy 实现。其 RegistrationResult API 和 v0.19.0 Registration.cpp 源码 对应上述口径。某些教程文本曾把 fitness 分母写成 target 点数，阅读时应核对版本与实现。本文代码会用实际对应数量自行检查分母和 RMSE，避免只凭名称猜含义。

这带来三个实际后果：交换 source 与 target 后，fitness 不一定相同；增大距离阈值可能把更多错误近邻算成有效对应；即使使用点到平面优化，结果中的 inlier_rmse 仍需要按该实现的欧氏近邻距离解释，不能直接当成点到平面的优化残差。

特别注意：在此版本实现中，没有对应点时，fitness 与 inlier_rmse 都可以为 0。RMSE=0 此时是空对应集合的返回约定，不是配准完美。必须先检查对应点数量，再讨论误差大小。

公平比较两种参数时，应尽量在同一份评价点集、相同距离阈值下重新计算指标。若一个结果用稀疏点、另一个用稠密点，或一个阈值宽、另一个阈值窄，直接比较两行数字很容易误导。

五、完整 Open3D 实验：两个满分分支，一个错误身份

5.1 实验设计与可复现环境

生成 600 个三维随机点组成一个点团，再将它绕 z 轴旋转 180°，复制成第二个点团，共 1200 点。这样得到的点集在半周旋转下保持不变。它是数学构造的点集，不模拟真实组织、真实扫描表面或某台设备的噪声。图中“表面重合”仅用于直观表达几何点集重合。

将点集绕 z 轴旋转 20°，再平移 (30,−12,8) mm，得到目标点云。设定三个未参与 ICP 的语义检查点，分别为 (35,10,5)、(25,−15,8)、(40,0,−5) mm；它们的真实目标位置用已知 T_true 计算。

比较三种初值：接近真实变换的初值；接近错误半周分支的初值；平移到很远、完全找不到对应点的初值。距离阈值固定为 4 mm，点到点刚体 ICP，最多 200 次迭代。随机种子固定为 20261015。

本文在独立 Python 3.11 环境中运行 Open3D 0.19.0。安装应使用与该版本支持情况匹配的解释器；若当前解释器找不到 wheel，不要直接认定包名有误。Windows 可按下列方式创建环境，其他系统用对应的 python3.11 命令：

py -3.11 -m venv .venv .venv\Scripts\python.exe -m pip install open3d==0.19.0 .venv\Scripts\python.exe icp_demo.py

保存以下完整代码为 icp_demo.py。只生成本地合成数据，不下载点云，不弹出可视化窗口；运行后生成 results.json 与 experiment.npz。NumPy 版本也写入结果文件，便于记录环境。

"""Synthetic ICP ambiguity demo. Coordinates are in millimetres.""" from pathlib import Path import json import numpy as np import open3d as o3d def transform(T, xyz): return xyz @ T[:3, :3].T + T[:3, 3] def pose(degrees, translation=(0, 0, 0)): a = np.deg2rad(degrees) T = np.eye(4) T[:3, :3] = [[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]] T[:3, 3] = translation return T def cloud(xyz): xyz = np.asarray(xyz, dtype=np.float64) if xyz.ndim != 2 or xyz.shape[1] != 3 or len(xyz) < 3: raise ValueError('Expected at least 3 points in an Nx3 array') if not np.isfinite(xyz).all(): raise ValueError('Non-finite coordinates') pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(xyz) return pcd def probe_rms(T, truth, probes): delta = transform(T, probes) - transform(truth, probes) return float(np.sqrt(np.mean(np.sum(delta * delta, axis=1)))) def run(source, target, initial, truth, probes, threshold=4.0): reg = o3d.pipelines.registration result = reg.registration_icp( source, target, threshold, initial, reg.TransformationEstimationPointToPoint(False), reg.ICPConvergenceCriteria( relative_fitness=1e-10, relative_rmse=1e-10, max_iteration=200)) T = result.transformation pairs = np.asarray(result.correspondence_set) # Do not interpret zero RMSE as success when no pairs exist. if len(pairs): a = transform(T, np.asarray(source.points))[pairs[:, 0]] b = np.asarray(target.points)[pairs[:, 1]] measured = np.sqrt(np.mean(np.sum((a - b)**2, axis=1))) assert np.isclose(measured, result.inlier_rmse, atol=1e-9) assert np.isclose(result.fitness, len(pairs) / len(source.points)) assert np.allclose(T[:3, :3].T @ T[:3, :3], np.eye(3), atol=1e-8) assert np.isclose(np.linalg.det(T[:3, :3]), 1.0) relative_R = T[:3, :3] @ truth[:3, :3].T angle = np.rad2deg(np.arccos(np.clip((np.trace(relative_R)-1)/2, -1, 1))) record = dict(fitness=float(result.fitness), inlier_rmse=float(result.inlier_rmse), pairs=int(len(pairs)), rotation_error_deg=float(angle), probe_rms_mm=probe_rms(T, truth, probes), transformation=T.tolist()) return record def main(): rng = np.random.default_rng(20261015) lobe = rng.normal(size=(600, 3)) * [4.0, 2.0, 3.0] + [20, 0, 0] symmetry = pose(180) xyz = np.vstack([lobe, transform(symmetry, lobe)]) truth = pose(20, [30, -12, 8]) target_xyz = transform(truth, xyz) source, target = cloud(xyz), cloud(target_xyz) # These three semantic probe points never enter ICP. probes = np.array([[35., 10., 5.], [25., -15., 8.], [40., 0., -5.]]) good_init = truth @ pose(3, [0.5, -0.2, 0.1]) wrong_init = truth @ pose(177, [0.5, -0.2, 0.1]) no_overlap = pose(0, [1000, 1000, 1000]) records = {name: run(source, target, initial, truth, probes) for name, initial in [('near_truth', good_init), ('near_half_turn', wrong_init), ('no_correspondences', no_overlap)]} good, wrong, empty = records.values() assert good['fitness'] > .999 and good['probe_rms_mm'] < 1e-6 assert wrong['fitness'] > .999 and wrong['inlier_rmse'] < 1e-6 assert wrong['probe_rms_mm'] > 50 and wrong['rotation_error_deg'] > 179 assert empty['pairs'] == 0 and empty['fitness'] == 0 root = Path(__file__).parent np.savez(root / 'experiment.npz', source=xyz, target=target_xyz, probes=probes, truth=truth, good=np.array(good['transformation']), wrong=np.array(wrong['transformation'])) output = dict(open3d=o3d.__version__, numpy=np.__version__, unit='mm', threshold=4.0, points=len(xyz), results=records) (root / 'results.json').write_text(json.dumps(output, indent=2), encoding='utf-8') for name, r in records.items(): print(name, 'fitness=%.6f RMSE=%.9f rotation=%.3f probe_RMS=%.6f' % (r['fitness'], r['inlier_rmse'], r['rotation_error_deg'], r['probe_rms_mm'])) print('PASS: metric formulas, rigid transforms, ambiguity and empty-pair checks') if __name__ == '__main__': main()

5.2 实际运行结果

运行环境：Open3D 0.19.0，NumPy 2.4.6。两组微小 RMSE 是浮点量级结果，表格按阈值展示，不能解释为真实扫描具备零噪声或无限精度。

图 3：真实运行数据的 XY 投影；检查点未参与拟合（原创教学图；实验为合成数据）

独立检查点 RMS 的定义是：把三个检查点分别通过估计变换与真实变换送到目标坐标系，再对三维位置差取均方根。图仅显示 XY 投影，表中误差使用完整三维坐标。这里已知检查点真值，因此能够评估目标变换误差；实际工程中还要考虑参考定位本身的不确定性。

5.3 这个实验能证明什么，不能证明什么？

错误分支将两个几何上完全相同的点团互换。近邻搜索仍然找到零距离点，因而几何分数无法区分两个分支；带有物理身份的检查点却被送到错误位置。这证明：纯近邻指标无法保证语义对应正确。

这不是对所有 ICP 场景失败率的统计，也不是一个“真实数据必然错 70.95 mm”的结论。误差数值来自人为设置的点位置与对称结构。更不能由此推断换成全局优化就能解决完全相同的几何解：如果输入信息不能区分身份，优化器没有依据替我们判断。

应对方式是引入可区分的信息，例如非对称结构、可靠纹理、独立标志、历史位姿或其他传感器约束。轻微破坏对称也不自动保证鲁棒，相关特征可能在下采样、遮挡或噪声中消失。

六、从合成示例走向真实点云：推荐的工程流程

6.1 先建立清晰的输入契约

保存原始数据，记录单位、坐标系、采集时间、有效范围和变换方向。读取之后检查空点云、NaN、Inf 和极端离群点；确认数据属于同一刚体、拥有足够共同结构。对设备导出的矩阵，先验证一个已知点的变换，避免方向反了还在调 ICP 参数。

6.2 先粗配准，再做局部精修

初始变换可以来自设备位姿、可靠标志点、人工粗对齐或特征匹配。Open3D 的 全局配准教程 提供了下采样、法向估计、FPFH 和 RANSAC，再做 ICP 精修的示例路径。这里“全局配准”是方法类别名称，不能自动理解为对任意数据都保证找到真实位姿。

FPFH 主要描述局部几何；如果大量结构相似，特征匹配仍会混淆。RANSAC 只能依据设定模型与检验条件筛选候选，不会凭空恢复遮挡区域中的身份信息。对明显对称对象，应保留多个合理候选并用额外证据消歧。

6.3 使用由粗到细的尺度，但明确它的作用

粗尺度减少数据量、放宽捕获范围；细尺度保留细节、降低最终对应距离。每一级都把上一级的 source→target 总变换作为 init，并保持同一个原始坐标定义。不要既手动把 source 变换一次，又把同一个矩阵作为 init 重复应用。

这些是流程建议，不是固定数值配方。毫米级小物体与米级房间不能复用同一组长度参数。多尺度提高了处理效率与可调性，但无法保证跳出错误分支，更不能消除完全对称导致的信息缺失。

6.4 点到平面接口如何接入？

下面是局部精修函数，使用同一 legacy API。它要求已得到可信初值，显式传入体素尺度与距离阈值，并在下采样后的目标上估计法向。参数单位必须与点坐标一致。它不是本文双叶实验的替代真值生成器，也没有在真实扫描数据上验证。

import numpy as np import open3d as o3d def refine_point_to_plane(source, target, initial, voxel, distance): if not np.isfinite([voxel, distance]).all() or min(voxel, distance) <= 0: raise ValueError('Positive finite voxel and distance required') T = np.asarray(initial, dtype=float) if T.shape != (4, 4) or not np.isfinite(T).all(): raise ValueError('Expected a finite 4x4 initial transform') if not np.allclose(T[3], [0, 0, 0, 1]): raise ValueError('Invalid homogeneous matrix') R = T[:3, :3] if not np.allclose(R.T @ R, np.eye(3), atol=1e-6) or not np.isclose(np.linalg.det(R), 1): raise ValueError('Initial transform must be rigid') for pcd in (source, target): points = np.asarray(pcd.points) if len(points) < 3 or not np.isfinite(points).all(): raise ValueError('Need at least 3 finite input points') src = source.voxel_down_sample(voxel) dst = target.voxel_down_sample(voxel) if min(len(src.points), len(dst.points)) < 10: raise ValueError('Too few points after downsampling') dst.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid( radius=3.0 * voxel, max_nn=50)) reg = o3d.pipelines.registration result = reg.registration_icp( src, dst, distance, T, reg.TransformationEstimationPointToPlane(), reg.ICPConvergenceCriteria(max_iteration=80)) if len(result.correspondence_set) < 10: raise RuntimeError('Too few correspondences; inspect initial pose and units') return result.transformation

函数返回的 T 仍然映射原始 source 坐标到 target 坐标，下采样不会改变坐标系。若需要展示结果，应复制原始点云后再调用 transform，避免原地修改输入造成后续重复变换。PointCloud 的 官方 API 可用于核对下采样、法向估计与变换操作。

6.5 鲁棒核解决的是异常残差，不是所有错误

鲁棒损失可以降低某些大残差点对对求解的影响，Open3D 鲁棒核教程 给出了相关点到平面流程。但几何上几乎零残差的错误对应，未必会被判为异常。核函数尺度同样需要匹配噪声；不能因为启用了“robust”就省略共同区域裁剪、初值检查或独立验证。

七、常见报错与看起来成功的失败

八、怎样证明结果值得使用？

图 4：从输入到独立证据的检查顺序（原创教学图；实验为合成数据）

第一层，检查算法是否完成有效计算：对应数足够吗，矩阵是否有限且接近刚体，残差主要出现在哪里？第二层，检查几何覆盖：在同一评估条件下查看正反向距离、有效点分布和多个初值结果。第三层，使用未参与拟合的语义检查点或外部参考，验证任务区域的位置误差。

双向距离有助于揭示覆盖不对称，却不是正确性的充分条件。本文的完全对称反例中，两个方向的几何距离都可以很小。多初值结果一致，也可能只是都进入了同一错误分支。每一项检查都应明确它能发现什么，以及仍可能漏掉什么。

用于独立验证的数据不能反复参与调参后仍被称为完全独立的最终测试集。若用检查点选择阈值或候选，最好另外保留最终验证数据；参考定位误差、时间同步误差和坐标链误差也要分别记录。医学影像和空间导航场景尤其不能把表面 RMSE 直接替代任务位置误差。

建议随结果保留：软件版本、原始数据标识、单位与坐标定义、ROI、下采样尺度、法向邻域、初值来源、距离阈值、迭代设置、对应点数、统一评价分数、候选变换与独立参考结果。这样后续定位偏差时，才能区分输入、模型、优化和验证各自的问题。

九、本文实验的验证与边界

完整代码已经实际运行，并用断言检查：fitness 分母与对应数一致；手工计算的欧氏 RMSE 与返回结果一致；旋转矩阵满足刚体约束；正确初值恢复检查点；半周分支低残差但检查点明显偏移；无重叠分支得到零对应。局部精修接口另在合成椭球面点云上做了执行检查。

本文没有评估真实扫描噪声、遮挡、动态点、各向异性误差、软组织形变、GPU 性能或不同 ICP 变体的总体优劣，也没有测量实际设备精度。合成实验用于拆解概念，真实项目仍应在有代表性的独立数据上验证。

参考资料

Open3D：ICP registration

Open3D：RegistrationResult API

Open3D v0.19.0：Registration.cpp 源码

Open3D：Global registration

Open3D：Robust kernels

Open3D：PointCloud API

写在最后：每一次迭代，都要知道自己在接近什么

“不积小流，无以成江海。”

—— 荀子，《劝学》

学习算法和调试工程一样，需要把小步骤积累起来：先认清坐标，再读懂指标，再验证假设。迭代次数可以很多，但方向要靠证据确认。名言原文参见 《荀子·劝学》。