4. 点云配准基础：ICP算法原理与变体

点云配准，说白了就是把不同位置拍到的点云拼到一起。你拿着激光扫描仪绕物体转一圈，每站扫到的点云都在自己的坐标系里，怎么把它们对齐到同一个坐标系？这就是配准要解决的问题。

ICP（Iterative Closest Point）是这里面最经典的算法。我从2015年开始做三维重建，前前后后调过不下十种ICP变体。说实话，很多项目最后能跑通，靠的就是把ICP吃透了。

4.1 ICP的核心思想

ICP的思路很直观：

  1. 找两个点云之间的对应点对
  2. 根据这些对应关系算一个变换矩阵
  3. 把源点云变换过去
  4. 重复直到收敛

嗯，这里要注意——ICP要求两个点云已经有粗略的对齐。如果初始位置差太远，它很容易掉进局部最优。我在项目里吃过这个亏，后面会讲怎么避坑。

ICP的收敛条件： 迭代次数达到上限 || 变换矩阵变化量小于阈值 || 均方误差变化小于阈值

4.2 点对点ICP（Point-to-Point ICP）

这是最原始的版本。它的目标函数是：

min(R,t) Σ || R·p_i + t - q_i ||²

其中p_i是源点云中的点，q_i是目标点云中对应的最近点。说白了就是让对应点之间的距离平方和最小。

求解方法：

  • 用SVD分解算旋转矩阵R
  • 用质心对齐算平移向量t
  • 交替迭代直到收敛

我记得第一次自己手写ICP时，SVD分解那步卡了很久。后来发现OpenCV和Eigen都有现成的函数，但理解原理还是很重要——至少你知道什么时候该用哪个变体。

我的经验： 点对点ICP对噪声敏感。如果点云有离群点，建议先用统计滤波滤一遍，否则误差会被少数坏点带偏。

4.3 点对面ICP（Point-to-Plane ICP）

点对点ICP只考虑点的位置，忽略了局部几何信息。点对面ICP改进了这一点——它让源点到目标点云对应点所在切平面的距离最小化。

目标函数变成：

min(R,t) Σ [ (R·p_i + t - q_i) · n_i ]²

其中n_i是目标点云中q_i点的法向量。

为什么更好？

  • 允许点在切平面上滑动，收敛更快
  • 对平坦区域的配准更鲁棒
  • 迭代次数通常比点对点少30%-50%

我曾经在一个室内场景重建项目里对比过两种方法。点对点ICP跑了80次迭代还没稳定，换成点对面ICP，25次就收敛了。差距就是这么明显。

注意： 点对面ICP需要计算法向量。法向量估计不准时，效果反而比点对点差。建议用PCA法向量估计，邻域半径选点云平均间距的3-5倍。

4.4 ICP的变体

实际项目中，标准ICP往往不够用。这里介绍两个我常用的变体。

4.4.1 Sparse ICP

标准ICP用L2范数（平方误差），对离群点非常敏感。Sparse ICP改用L1范数或Huber损失，说白了就是让大误差的权重降低。

目标函数：

min(R,t) Σ ρ( ||R·p_i + t - q_i|| )

其中ρ是鲁棒核函数。

适用场景：

  • 点云有大量离群点
  • 部分重叠率低（低于50%）
  • 动态场景中有移动物体

我做过一个隧道扫描的项目，点云里有很多飞点（激光打到水雾上产生的噪声）。标准ICP完全跑偏，换成Sparse ICP后结果就对了。嗯，这里的关键是核函数参数要调——太敏感等于没用，太鲁棒又收敛慢。

4.4.2 Generalized ICP

Generalized ICP把点对点和点对面统一到了一个框架里。它用协方差矩阵来描述每个点的局部几何特性：

  • 如果点是平面上的点（法向量明确），用点对面约束
  • 如果点是角点或边缘（法向量不确定），用点对点约束
  • 自动根据局部几何结构分配权重

说白了就是让算法自己判断该用哪种方式。我在做室外大场景重建时特别喜欢用这个变体——场景里既有墙面（平面）又有树干（圆柱），Generalized ICP能自动适配。

三种ICP对比：
特性 点对点ICP 点对面ICP Generalized ICP
收敛速度 慢 快 快
对噪声鲁棒性 低 中 高
对离群点鲁棒性 低 低 中
需要法向量 否 是 是
实现复杂度 低 中 高

4.5 配准评价指标

配准做完了，怎么知道结果好不好？不能光靠肉眼瞅。我一般用以下几个指标：

4.5.1 均方根误差（RMSE）

RMSE = sqrt( (1/N) Σ ||R·p_i + t - q_i||² )

这个指标最常用。但要注意——它只计算了有对应关系的点对。如果两个点云重叠率低，RMSE可能虚低。

4.5.2 对应点距离中位数（Median Error）

比RMSE更鲁棒。离群点对中位数影响小。我一般两个都看，如果RMSE和中位数差距很大，说明有离群点没处理好。

4.5.3 重叠率（Overlap Ratio）

Overlap = (距离小于阈值的对应点对数) / 总点数

阈值一般取点云平均间距的2-3倍。重叠率低于30%的配准基本不可靠。

4.5.4 旋转误差与平移误差

如果有真值（比如仿真数据或高精度标定），可以单独算：

  • 旋转误差：R_est和R_gt之间的角度差
  • 平移误差：t_est和t_gt之间的欧氏距离
我的习惯： 实际项目中，我会把配准后的点云叠加显示，用不同颜色区分源和目标。如果能看到清晰的"重影"或"错层"，哪怕RMSE再低，这个配准也是失败的。视觉检查永远是最后一道关。

4.6 ICP的知识体系

下面这张图总结了ICP的核心脉络：

ICP算法知识体系 ICP算法知识体系 ICP核心思想 点对点ICP 点对面ICP Generalized ICP SVD求解R,t L2范数最小化 切平面距离最小化 需要法向量估计 协方差矩阵建模 自适应权重分配 Sparse ICP 其他变体 L1/Huber鲁棒核 抗离群点能力强 RMSE 中位数误差 重叠率 避坑提示 初始对齐不好 → 局部最优 法向量不准 → 点对面失效

4.7 避坑指南

最后分享几个我踩过的坑：

  • 初始对齐太差： 我曾经直接把两站相距10米的点云丢进ICP，结果跑了200次迭代还是错的。后来先用FPFH特征匹配做粗配准，再用ICP精配准，问题就解决了。
  • 点云密度不均匀： 近处点密、远处点稀，ICP会偏向密集区域。建议做均匀降采样，或者用体素滤波让密度均匀。
  • 对称结构误导： 圆柱、球体这类对称物体，ICP可能收敛到错误的角度。我遇到过把圆柱转了180度还自认为配准成功的情况。解决办法是加入几何约束或人工标记点。
  • 迭代次数设太少： 默认50次不一定够。我一般设100-200次，加一个early stopping——如果连续5次误差变化小于1e-6就提前退出。
重要提醒： ICP不是万能的。如果两个点云重叠率低于30%，或者初始旋转误差超过30度，先做粗配准再回来用ICP。否则就是在浪费时间。
← 上一章 📖 返回目录 下一章 →