ICP 算法 — Point-to-Point, Point-to-Plane, GICP, KISS-ICP
- Description：Iterative Closest Point 算法 — 已知/未知对应关系的两种处理，闭式 SVD 解 vs 非线性优化，进阶变体 (Point-to-Plane / GICP / NDT / KISS-ICP)
- Source：从 [K2E-2] SLAM Common (Notion id
fb32092b-1e02-4220-9f09-8d991febb891) 的 ICP 提及拆分；原 note 仅一句 "ICP 的解法一共有两种：SVD 方法和非线性优化方法"，本文从教材与原论文重写 - Created：2023-04-10
- Updated：2026-06-03
- License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文
Table of Contents
- 1. ICP 问题
- 2. 对应已知 → 闭式 SVD 解 (Point-to-Point)
- 3. 对应未知 → 迭代最近点
- 4. 非线性优化解 (Point-to-Plane 及更广义)
- 5. 数据关联与加速
- 6. 进阶变体
- 7. 实务陷阱
- 8. 速查对比
- References
1. ICP 问题
Iterative Closest Point — 给定两组 3D 点云 ${p_i} \subset \mathbb{R}^3$ (源 source) 和 ${q_i} \subset \mathbb{R}^3$ (目标 target)，求刚体变换 $(R, t) \in SE(3)$ 使得：
$$ (R^, t^) = \arg\min_{R, t} \sum_{i=1}^{N} | q_i - (R p_i + t) |^2 $$
两类子问题：
| 子问题 | 描述 | 解法 |
|---|---|---|
| 对应已知 | 知道哪个 $p_i$ 对应哪个 $q_i$ (如特征点匹配后) | 闭式 SVD 解，一步到位 |
| 对应未知 | 只有两堆点，不知道哪个对哪个 | 迭代：估对应 → 解 $(R,t)$ → 更新 → 重复 |
后者就是 "Iterative" 的来源。
典型场景：
- 激光雷达里程计 (LiDAR odometry) — FAST-LIO、LOAM、KISS-ICP
- 点云地图融合 / 回环对齐
- PnP 中 EPnP 最后一步 (3D 控制点配准 — 对应已知，属 §2 闭式 SVD 解，非迭代 ICP)
- RGB-D 帧间配准 — KinectFusion 用 Point-to-Plane ICP
- 多视角扫描的全局配准 — 先粗对齐再 ICP 精对齐
2. 对应已知 → 闭式 SVD 解 (Point-to-Point)
Arun 1987 给出的 SVD 闭式解 (本文 §2 推导即此法)；Horn 1987 是等价问题的另一种闭式解 (用单位四元数 / 4×4 矩阵特征向量，算法不同)。
步骤
1. 去中心化
计算两组点云的质心 (centroid)：
$$ \bar{p} = \frac{1}{N} \sum_i p_i, \quad \bar{q} = \frac{1}{N} \sum_i q_i $$
去中心化坐标：$p'_i = p_i - \bar{p}$，$q'_i = q_i - \bar{q}$。
2. 构造协方差矩阵 $H$
$$ H = \sum_{i=1}^{N} p'_i , q'^T_i \in \mathbb{R}^{3 \times 3} $$
3. SVD 分解
$$ H = U \Sigma V^T $$
4. 求解 $R, t$
$$ R = V U^T $$
如果 $\det(R) = -1$ (反射矩阵)，把 $V$ 最后一列取负号重新计算 — 同旋转与刚体变换那篇 (§4 旋转矩阵的性质) 里说的右手系修正一致。
$$ t = \bar{q} - R \bar{p} $$
为什么这样能解？
把目标函数展开：
$$ \sum_i | q_i - R p_i - t |^2 $$
对 $t$ 求偏导为零 → $t = \bar{q} - R \bar{p}$ (代回消掉 $t$)
剩下的关于 $R$ 的优化等价于：
$$ \max_R \text{tr}(R H), \quad R \in SO(3) $$
这是经典的 Orthogonal Procrustes 问题，SVD 闭式解为 $R = V U^T$。
优缺点
- 优点：一步到位，无迭代，全局最优
- 缺点：必须知道对应关系；对外点敏感 (一个错误对应能拉偏整个 $R$)
- 改进：配合 RANSAC 抗外点；或加 Huber/Cauchy 鲁棒核
3. 对应未知 → 迭代最近点
经典 ICP 算法 (Besl & McKay 1992) — 用最近邻搜索逐步建立对应，迭代收敛。
算法
输入: 源点云 P = {p_i}, 目标点云 Q = {q_j}, 初值 T_0 = (R_0, t_0)
T ← T_0
repeat:
1. 对每个 p_i，在 Q 中找最近邻 q_{NN(i)} 作为对应点
2. 用 §2 闭式 SVD 解出当前对应下的最优 (R*, t*)
3. 更新 T ← (R* T_R, R* t_T + t*) (即变换累积)
4. 把 T 应用到源点云: p_i ← R* p_i + t*
until 收敛 (位姿变化 < ε 或 残差 < δ 或 迭代次数上限)
收敛性
保证收敛到局部最优：每次迭代残差单调不增 (因为对应步和求解步各自单调改进目标函数)。
不保证全局最优：初值差时容易陷局部极小 (常见现象：点云配对 "卡" 在错误对应模式上)。
实务对策：
- 用其他方法 (NDT, FPFH+RANSAC, FGR) 做粗对齐 → ICP 精对齐
- 多次随机初值
- 使用稳健核函数减小局部误对应权重
4. 非线性优化解 (Point-to-Plane 及更广义)
Point-to-point ICP 把每个对应当成 "点对点距离" 来优化。Point-to-Plane (Chen & Medioni 1991) 改成 "点到目标点切平面的距离"：
$$ E_{\text{plane}} = \sum_i \left( (q_i - R p_i - t) \cdot n_i \right)^2 $$
其中 $n_i$ 是 $q_i$ 所在局部平面的法向量 (估计自 $q_i$ 邻域)。
为什么更好：
- 几何上 — 实际表面是平滑的，"点 vs 切面" 比 "点 vs 点" 更符合真实约束
- 数值上 — 收敛更快 (Rusinkiewicz-Levoy 2001 / Low 2004 实验均显示 point-to-plane 比 point-to-point 收敛显著更快，但无统一倍数)
- 抗误对应 — 法向量约束让滑动方向上的误差不被惩罚 (允许 "沿表面滑")
求解：无全局闭式解。常用小角度线性化 (Low 2004) 把每次迭代化为 6×6 线性最小二乘 (Low 2004 用 SVD 伪逆 $A^+ = V\Sigma^+ U^T$ 解；工程上也常用正规方程 + Cholesky 加速；每次迭代有闭式)，外层仍迭代更新对应；或直接用 GN/LM 求 $(R, t)$ — 同 PnP §6 的雅可比形式。
进一步泛化：GICP
Generalized ICP (Segal 2009) 把 point-to-point 和 point-to-plane 统一在概率框架下：
$$ E_{\text{GICP}} = \sum_i (q_i - R p_i - t)^T , \Sigma_i^{-1} , (q_i - R p_i - t) $$
其中 $\Sigma_i$ 是融合源点和目标点局部协方差的矩阵：
$$ \Sigma_i = \Sigma_{q,i} + R \Sigma_{p,i} R^T $$
- 设 $\Sigma_p = 0$, $\Sigma_q$ 为各向同性 → 退化为 point-to-point
- 设 $\Sigma_q$ 沿法向量方向为 0、其他方向为 $\infty$ → 等价 point-to-plane
- 实际取 $\Sigma_q$ 为局部协方差的椭球 → 自然处理各向异性表面
5. 数据关联与加速
ICP 每次迭代的核心瓶颈是最近邻搜索。$N$ 个源点 × $M$ 个目标点的暴力搜索是 $O(NM)$。
加速结构：
- KD-tree — 经典选择，搜索 $O(N \log M)$。PCL 默认实现。
- iVox / ikd-Tree — FAST-LIO2 用的增量结构，避免重建整棵树
- Voxel-based 哈希 — KISS-ICP 直接对体素查最近邻，省去复杂数据结构
- GPU 并行 — 大规模点云用 GPU 做暴力搜索仍可能比 CPU KD-tree 快
6. 进阶变体
NDT (Normal Distributions Transform)
不是严格的 ICP，但思路相近。把目标点云划分体素，每个体素拟合一个高斯，源点 $p_i$ 的代价是它落到目标体素高斯的负对数似然：
$$ E_{\text{NDT}} = -\sum_i \log \mathcal{N}(R p_i + t \mid \mu_{v(i)}, \Sigma_{v(i)}) $$
- 优点：不用显式最近邻搜索，对密度差异鲁棒
- 缺点：体素分辨率敏感
经典应用：自动驾驶激光雷达定位 (Autoware 用 NDT)。
ICP 的鲁棒变体
| 变体 | 加什么 | 解决问题 |
|---|---|---|
| Trimmed ICP | 每轮只用残差最小的前 $\alpha%$ 对应 | 抗外点 |
| Sparse ICP | $L_p$ 范数 ($p < 1$) 替代 $L_2$ | 抗外点 |
| Robust ICP | Huber/Cauchy 核 | 大误差对应降权 |
| Go-ICP | Branch-and-Bound 全局搜索 | 不要求好初值 |
KISS-ICP (Vizzo 2023)
最近的极简主义 LiDAR 里程计 — point-to-point ICP + 自适应阈值 + 帧间位姿外推作初值。论文核心结论：经典 ICP 调好工程细节，不需要复杂变体就能打过很多深度学习方法。
7. 实务陷阱
- 初值 — ICP 是强初值敏感算法。LiDAR 里程计实际用恒速模型外推上一帧位姿做初值；视觉 RGB-D 用前一帧 + IMU 预积分初值。
- 外点 — 默认 ICP 假设所有点都有正确对应，但点云常有遮挡/动态物体 → 必须配合稳健核或 trim 策略
- 退化场景 — 长走廊、空旷场地里 ICP 沿运动方向不可观 → 几何上欠约束，会漂。LIO-SAM/FAST-LIO 用 IMU 弥补。
- 法向量估计 (point-to-plane) — 需要稳定的局部邻域；点云稀疏时法向不准
- scale 问题 — ICP 解的是刚体变换，不解尺度。如果两个点云尺度不一致 (单目 SLAM 跨尺度配准)，需要 Sim(3) ICP 或先归一化
8. 速查对比
| 方法 | 代价函数 | 解法 | 优点 | 缺点 |
|---|---|---|---|---|
| Point-to-Point ICP | $\sum | q_i - R p_i - t |^2$ | SVD 闭式 (已知对应) / 迭代 | 简单，全局最优 (已知对应) | 慢收敛，对噪声敏感 |
| Point-to-Plane ICP | $\sum ((q_i - Rp_i - t) \cdot n_i)^2$ | 线性化 6×6 闭式 (Low 2004) / GN·LM | 收敛更快，几何更对 | 需要法向估计 |
| GICP | $\sum r_i^T \Sigma_i^{-1} r_i$ | GN / LM | 概率框架，统一表达 | 计算量大 |
| NDT | $-\sum \log \mathcal{N}$ | GN / LM | 无需 KNN，鲁棒 | 体素分辨率敏感 |
| KISS-ICP | Point-to-Point + 自适应阈值 | 迭代 | 工程上极简且强 | 仍是局部最优 |
References
- Besl, P. J., & McKay, N. D. (1992). A Method for Registration of 3-D Shapes. IEEE T-PAMI, 14(2). — ICP 原始论文
- Arun, K. S., Huang, T. S., & Blostein, S. D. (1987). Least-Squares Fitting of Two 3-D Point Sets. IEEE T-PAMI, 9(5), 698–700. — SVD 闭式解 (本文 §2 的方法)
- Horn, B. K. P. (1987). Closed-form Solution of Absolute Orientation Using Unit Quaternions. JOSA A, 4(4). — 四元数/特征向量闭式解 (与 SVD 法等价、算法不同)
- Chen, Y., & Medioni, G. (1991). Object Modeling by Registration of Multiple Range Images. Proc. IEEE ICRA, 2724–2729. — Point-to-Plane ICP (会议版)
- Chen, Y., & Medioni, G. (1992). Object Modeling by Registration of Multiple Range Images. Image and Vision Computing, 10(3), 145–155. — Point-to-Plane ICP (期刊版)
- Segal, A., Haehnel, D., & Thrun, S. (2009). Generalized-ICP. RSS. — GICP
- Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform. PhD thesis. — NDT
- Yang, J., Li, H., & Jia, Y. (2013). Go-ICP: Solving 3D Registration Efficiently and Globally Optimally. ICCV. — Branch-and-Bound 全局 ICP
- Vizzo, I., Guadagnino, T., Mersch, B., Wiesmann, L., Behley, J., & Stachniss, C. (2023). KISS-ICP: In Defense of Point-to-Point ICP – Simple, Accurate, and Robust Registration. RA-L. — 极简主义 LiDAR ICP
- 高翔. 视觉 SLAM 十四讲 (第 2 版). 第 7 章 §7.9 (3D-3D: ICP) — 中文教材入门
- Pomerleau, F., Colas, F., & Siegwart, R. (2015). A Review of Point Cloud Registration Algorithms for Mobile Robotics. Foundations and Trends in Robotics. — ICP 全景综述