ICP 失败的原因：初始化、异常值和对称几何

Contents — find the section you need

Change parameters and verify

Open the panel, then press Run to load Python. You can stop execution and reset parameters. Results are computed on this device. No Python installation is required.

Local execution steps below are optional for reproducing the source results; they are not required for the browser experiment.

The experiment controls are in English.

Open experiment panel in a new tab

Download reproduction source

ICP算法交替使用最近邻匹配和刚体变换估计。残差越小，恢复的姿态就越正确。简单的二维形状可以帮助我们隔离初始化、异常值和对称性，而无需考虑传感器噪声。

目标

此实现旨在最小化点到点距离的平方。每次迭代都会为变换后的源点找到目标邻域，将匹配点保持在距离阈值内，并通过奇异值分解（SVD）估计旋转/平移。行列式校正可以防止反射。算法最多迭代100次后停止。Open3D ICP教程区分了点到点和点到平面的目标；此处仅实现了点到点的目标。

输入和执行

下载脚本并运行python3 engineering_labs.py。已使用 Python 3.12.3、NumPy 1.26.4 和 Matplotlib 3.6.3 进行测试。无需 Open3D，且本次测试并非与 Open3D 实现进行速度比较。

目标是一个由 3 米和 2 米线段组成的 L 形，共 102 个点，其中一个角点重复。源点由真实值 20° 旋转的倒数和 (0.4, -0.3) 米的平移生成。未添加传感器噪声。理想初始值为 18° 和 (0.35, -0.25) 米；不良初始值为 110° 和 (1.5, 1) 米。

读取三种证据

残差是指在可接受的最近邻距离上的均方根误差 (RMSE)。覆盖率是指所有源点中位于阈值范围内的点的比例。合成真实值也允许存在旋转和平移误差。

条件	阈值 [米]	残差 [米]	覆盖率	旋转误差 [°]	平移误差 [m]
良好初始化	0.5	0.023	1.000	1.258	0.055
不良初始化	0.5	0.151	0.539	171.699	2.519
30 个异常值，宽阈值	10	1.117	1.000	71.412	2.657
30 个异常值，窄阈值	0.5	0.023	0.773	1.258	0.055
对称圆，初始旋转 90°	0.5	约等于 0	1.000	90.000	约 0

即使初始化良好，仍然存在误差：规则采样线段上的相邻点允许局部解发生偏移。仅覆盖率 1.000 无法检测到该误差。异常值位于源 x=4–7 米，y=3 米处。下载目标、异常值输入/输出和摘要。

圆和墙体中无法观测的方向

圆的半径为 1 米，包含 120 个点，其恒等变换定义为真实变换。旋转 90° 后，其点集仍然匹配，因此残差基本为零，同时存在 90° 的位姿误差。更多迭代无法补充缺失的信息。

墙体带来了一个相关但不同的问题。点到面误差测量的是沿墙体法线的位移，对切向运动的约束较差。有限的点到点墙体可以从端点和点的位置获取信息，但在没有可见端点的长走廊中，这种方法会变得很弱。圆是一个已执行的实验；关于墙体的讨论解释了目标的方向约束。

更小的阈值并非万能的解决方案

0.5 米的阈值可以去除这些异常值，但当初始化距离较远时，也可能消除正确的对应关系。不同的阈值会选择不同的点，因此仅凭残差无法直接比较。此代码在接受的点少于三个时停止更新；三个点本身并不能保证良好的几何形状。

实际可行的方案包括里程计初始化、由粗到精的配准、异常值过滤以及观察不同方向的表面。在相同的时间间隔和参考条件下测量它们的效果。

继续进行 SLAM 评估

这是单帧 2D 配准，并非 3D 驾驶精度或运动物体鲁棒性测试。SLAM 评估添加了轨迹误差、时间对齐和失败区间。首先仅改变初始角度，以观察为何无法用一个残差值概括收敛情况。

在本地运行相同的计算

解压完整的可运行源代码 ZIP，或将实验入口点、生成的内核和原始 PID 源代码保存在一个文件夹中。PID 使用 Python 标准库；Kalman 和 ICP 需要 NumPy。原始图形生成命令仍然可用。

python3 -m pip install numpy==1.26.4
python3 experiment.py icp --parameters '{"case":"good_init","threshold":0.5}' > result.json

尝试 bad_init、outliers 和 symmetric_ring。比较 outliers 的阈值 10 和 0.5。验证即使残差接近于零，该环仍能保持 90 度旋转误差。

Code, data and reproduction

The ZIP contains the same computation code as the browser, synthetic data and comparison cases. These are not physical measurements.

Download reproduction package · Instructions (EN/JA) · Files and experiment conditions (JSON)

Extract the ZIP into a new directory, then run:

python3 reproduce.py --experiment icp

This Lab compares 5 cases. PID uses Python’s standard library; Kalman/ICP require NumPy. See the instructions for dependencies and tested versions.