第3章：ICP及其变种算法
说到激光雷达定位，ICP（Iterative Closest Point）绝对是个绕不开的话题。我记得刚入行那会儿，觉得ICP不就是个点云配准嘛，有啥难的？结果第一次在实际项目中跑经典ICP，直接给我上了一课——漂移、陷入局部最优、计算慢，问题一个接一个。后来我才明白，ICP这玩意儿，看似简单，里面的门道可深了。
这一章，咱们就来好好聊聊ICP及其各种变种。我会从最经典的ICP原理讲起，然后逐步深入到Point-to-Plane、Point-to-Line、GICP、NICP这些改进版本。说白了，就是带你看看各路大神是怎么在原始ICP基础上“打补丁”的。
3.1 经典ICP算法原理与推导
ICP的核心思想其实很朴素：给定两片点云，源点云和目标点云，我们想找到一个刚体变换（旋转+平移），让源点云经过变换后，能跟目标点云“对齐”。
嗯，这里要注意，ICP是个迭代算法。它每次做两件事：找对应点和求解变换。这两步反复交替，直到收敛。
3.1.1 数学建模
假设我们有源点云 P = {p₁, p₂, ..., pₙ} 和目标点云 Q = {q₁, q₂, ..., qₙ}。我们希望找到一个旋转矩阵 R 和平移向量 t，使得：
min Σ || R·pᵢ + t - qᵢ ||²这个目标函数，说白了就是所有对应点对之间的欧氏距离平方和最小。你想想看，如果变换完美，每个源点经过变换后应该正好落在对应的目标点上，距离为0。
3.1.2 SVD求解法
求解这个优化问题，最经典的方法就是SVD（奇异值分解）。我个人习惯用这种方法，因为它数值稳定性好，而且实现起来也不复杂。
具体步骤是这样的：
- 计算源点云和目标点云的质心：
μₚ = (1/n) Σ pᵢ，μₙ = (1/n) Σ qᵢ - 去中心化：
p'ᵢ = pᵢ - μₚ，q'ᵢ = qᵢ - μₙ - 构建协方差矩阵：
W = Σ p'ᵢ · q'ᵢᵀ - 对W进行SVD分解：
W = U·Σ·Vᵀ - 得到旋转矩阵：
R = V·Uᵀ - 得到平移向量：
t = μₙ - R·μₚ
关键点：如果 det(V·Uᵀ) = -1，说明我们得到了一个反射变换，需要特殊处理。我在项目中遇到过这种情况，当时排查了好久才发现是SVD分解后忘了检查行列式符号。
3.1.3 经典ICP的局限性
经典ICP虽然简单，但问题也很明显：
- 对初始位姿敏感：如果初始位置差太远，很容易陷入局部最优
- 只考虑点对点的距离：在结构化场景中，这种度量方式不够鲁棒
- 计算量大：每次迭代都要找最近邻，复杂度是O(n²)
我曾经在一个隧道场景里跑经典ICP，因为初始位姿偏差了5米，结果算法直接收敛到了墙上。嗯，从那以后我就学乖了，要么先用IMU做粗对齐，要么换个更鲁棒的变种。
3.2 Point-to-Plane ICP
Point-to-Plane ICP是经典ICP的一个直接改进。它不再最小化点到点的距离，而是最小化点到平面的距离。
你想想看，在室内场景中，墙面、地面都是平面。如果只用点到点的距离，一个点在墙面上滑动时，距离变化可能不大，导致优化方向不明确。但如果是点到平面的距离，沿着平面方向移动时，距离不变，垂直于平面方向移动时，距离变化明显——这就给了优化算法更明确的梯度信息。
3.2.1 目标函数
min Σ || (R·pᵢ + t - qᵢ) · nᵢ ||²其中 nᵢ 是目标点 qᵢ 处的法向量。说白了，就是让源点到目标点所在平面的距离最小化。
3.2.2 求解方法
这个优化问题没法像经典ICP那样直接求闭式解，需要用非线性最小二乘法。我一般用高斯-牛顿法或者LM算法来求解。
具体来说，我们把旋转矩阵用李代数参数化，然后线性化目标函数，迭代求解。嗯，这里涉及一些李群李代数的知识，不过你暂时可以把它理解成：把旋转矩阵用三个参数表示，然后做泰勒展开。
实战建议：Point-to-Plane ICP在结构化场景中效果非常好，比如室内走廊、停车场。但在野外或者森林场景中，法向量估计本身就不准，效果反而可能不如经典ICP。
3.3 Point-to-Line ICP
Point-to-Line ICP，顾名思义，最小化的是点到直线的距离。这个变种在2D激光雷达中特别常见，但在3D场景中也有应用。
我记得有一次做AGV的定位，用的是2D激光雷达。走廊场景中，墙面是直线，用Point-to-Line ICP效果出奇的好。为什么？因为沿着走廊方向，点到直线的距离不变，垂直于走廊方向，距离变化明显——这正好对应了AGV在走廊中的运动自由度。
3.3.1 目标函数
min Σ || (R·pᵢ + t - qᵢ) × dᵢ ||²其中 dᵢ 是目标点 qᵢ 处的直线方向向量。这个目标函数表示的是点到直线的垂直距离。
3.4 GICP（广义ICP）
GICP，全称Generalized ICP，是Alex Segal等人在2009年提出的。它的核心思想是：把经典ICP和Point-to-Plane ICP统一到一个概率框架下。
说白了，GICP不再假设点云是精确的测量值，而是认为每个点都服从一个高斯分布。这样，我们就能同时考虑点的位置不确定性和法向量信息。
3.4.1 概率模型
在GICP中，我们假设源点云和目标点云中的每个点都服从高斯分布：
pᵢ ~ N(p̂ᵢ, Cₚᵢ)
qᵢ ~ N(q̂ᵢ, Cₙᵢ)其中 Cₚᵢ 和 Cₙᵢ 是协方差矩阵，反映了点的不确定性。在平面区域，协方差矩阵在法线方向上的方差很小，在切线方向上的方差很大。
3.4.2 目标函数
min Σ || R·pᵢ + t - qᵢ ||² / (Cₙᵢ + R·Cₚᵢ·Rᵀ)这个目标函数其实是在最大化概率似然。当协方差矩阵取特定形式时，GICP可以退化为经典ICP或Point-to-Plane ICP。
GICP的优势：它不需要显式地估计法向量，而是通过协方差矩阵隐式地编码了局部几何信息。这使得GICP在各种场景中都有不错的鲁棒性。
3.5 NICP（法线ICP）
NICP，全称Normal ICP，是另一种思路。它不光考虑点的位置，还考虑点的法向量。在找对应点时，NICP要求对应点对不仅距离近，法向量也要相似。
你想想看，如果两个点距离很近，但法向量方向完全相反，它们很可能不是真正的对应点。NICP通过引入法向量约束，大大减少了错误匹配的概率。
3.5.1 匹配准则
NICP的匹配准则包含两部分：
- 位置约束：|| pᵢ - qⱼ || < ε₁
- 法向量约束：|| nₚᵢ - nₙⱼ || < ε₂
只有同时满足这两个条件的点对，才被认为是有效对应点。
3.5.2 目标函数
min Σ || R·pᵢ + t - qᵢ ||² + α · || R·nₚᵢ - nₙᵢ ||²其中α是权重系数，平衡位置误差和法向量误差。我在项目中一般把α设为0.1，效果还不错。
注意：NICP对法向量估计的质量非常敏感。如果传感器噪声大，或者点云稀疏，法向量估计不准，NICP的效果反而会变差。我曾经在低线束激光雷达上试过NICP，结果还不如经典ICP。
3.6 各算法对比
说了这么多，咱们来做个对比总结。下面这个表格是我根据自己的实战经验整理的：
| 算法 | 度量方式 | 收敛速度 | 鲁棒性 | 适用场景 |
|---|---|---|---|---|
| 经典ICP | 点到点 | 慢 | 低 | 简单场景，初始位姿好 |
| Point-to-Plane | 点到平面 | 中 | 中 | 结构化场景（室内、城市） |
| Point-to-Line | 点到直线 | 中 | 中 | 2D激光雷达，走廊场景 |
| GICP | 概率距离 | 快 | 高 | 通用场景，鲁棒性好 |
| NICP | 位置+法向量 | 中 | 高 | 高精度法向量估计的场景 |
3.7 知识体系结构图
下面这张图展示了ICP及其变种算法的知识体系结构，方便你从整体上把握：
3.8 代码实现示例
最后，我给出一个经典ICP的Python实现片段。虽然实际项目中我们一般用PCL或者Open3D，但理解底层实现还是很有必要的。
import numpy as np
from scipy.spatial import KDTree
def icp(source, target, max_iter=50, tolerance=1e-6):
"""
经典ICP算法实现
source: 源点云 (N, 3)
target: 目标点云 (M, 3)
"""
# 初始化变换矩阵
R = np.eye(3)
t = np.zeros((3, 1))
# 构建KD树用于最近邻搜索
tree = KDTree(target)
prev_error = 0
for i in range(max_iter):
# 1. 变换源点云
transformed = (R @ source.T + t).T
# 2. 找对应点
distances, indices = tree.query(transformed)
# 3. 获取对应点对
src_pts = source
tgt_pts = target[indices]
# 4. 计算质心
src_centroid = np.mean(src_pts, axis=0)
tgt_centroid = np.mean(tgt_pts, axis=0)
# 5. 去中心化
src_centered = src_pts - src_centroid
tgt_centered = tgt_pts - tgt_centroid
# 6. SVD求解
W = src_centered.T @ tgt_centered
U, _, Vt = np.linalg.svd(W)
R_new = Vt.T @ U.T
# 检查反射
if np.linalg.det(R_new) < 0:
Vt[-1, :] *= -1
R_new = Vt.T @ U.T
t_new = tgt_centroid.reshape(-1, 1) - R_new @ src_centroid.reshape(-1, 1)
# 7. 更新变换
R = R_new @ R
t = R_new @ t + t_new.reshape(-1, 1)
# 8. 检查收敛
mean_error = np.mean(distances)
if abs(prev_error - mean_error) < tolerance:
break
prev_error = mean_error
return R, t实战小贴士：在实际项目中，我一般不会直接用这个裸写的ICP。Open3D的ICP实现已经非常成熟了，而且支持多种变种。但理解这个底层实现，能帮你更好地调参和debug。
好了，这一章的内容就到这里。ICP及其变种是激光雷达定位的基石，理解了它们，后面的NDT、LOAM等算法学起来就会轻松很多。记住，没有最好的算法，只有最适合场景的算法。选型的时候，多想想你的传感器特性、场景结构和计算资源限制。