最近做 3D LiDAR 重定位、点云地图匹配和 ROS 2 导航的时候，又绕不开一个经典问题：

ICP、NDT、GICP 到底有什么区别？

刚开始我以为它们都是“把当前点云和地图对齐”的不同写法，后来才发现不是。它们确实都在干点云配准，但底层思想不一样，对初值的依赖不一样，适合的场景也不一样。

简单说：

方法	核心思想	优点	缺点
ICP	找最近点，不断迭代对齐	简单直观，适合精配准	很依赖初值，容易局部最优
NDT	把地图划成网格，每个网格建高斯分布	对 LiDAR 定位比较友好	voxel 分辨率敏感
GICP	在 ICP 基础上加入局部协方差	精度更高，适合细配准	计算量更大，实现更复杂

一句话概括：

ICP 像是“我找最近的点跟你对齐”；NDT 像是“我把地图变成概率场，看你落在哪个地方更合理”；GICP 像是“我不仅看点近不近，还看周围结构像不像”。

---

1. ICP：最经典，也最容易让我怀疑人生

ICP，全称是 Iterative Closest Point，也就是迭代最近点算法。

它的思路非常直接：

1. 给定两帧点云：source 和 target；

2. 对 source 中的每个点，在 target 中找最近点；

3. 根据这些点对，计算一个最优位姿变换；

4. 用这个变换更新 source；

5. 重复迭代，直到误差收敛。

这套流程听起来很朴素，甚至有点“这不就是暴力对齐吗”的感觉。

但 ICP 最大的问题也非常明显：

它非常依赖初值。

如果初始位姿已经比较接近真实位置，ICP 通常表现不错，可以把点云进一步精细对齐。

但如果初始位姿差得很远，它可能会非常自信地收敛到一个错误位置。

最痛苦的是，它不是不收敛。
它会收敛。
只是收敛到哪里就不好说了。

这就像我一开始调点云配准，看终端输出 convergence，以为成功了。结果打开 RViz 一看，机器人已经贴墙飞行了。

算法：我收敛了。
我：你收敛到哪去了？

---

2. ICP 的两种常见形式

ICP 里面常见的形式主要有两类：

2.1 Point-to-Point ICP

Point-to-Point ICP 是最基础的 ICP。

它的目标就是让 source 点经过变换之后，尽量靠近 target 中对应的最近点。

可以粗略理解为：

source 点变换后，离 target 最近点越近越好

这种方法简单直接，但对噪声、点云密度和错误匹配比较敏感。

如果点云比较稀疏，或者两个点云之间差异比较大，Point-to-Point ICP 很容易匹配得不稳定。

---

2.2 Point-to-Plane ICP

Point-to-Plane ICP 不再只看点到点的距离，而是看 source 点到 target 局部平面的距离。

简单理解：

不是要求两个点完全贴在一起，而是要求 source 点尽量落到 target 的局部平面上

这在结构化场景里很有用，比如：

• 地面；

• 墙面；

• 走廊；

• 建筑物表面；

• 室内外规则环境。

因为真实 LiDAR 点云里，很多点并不是孤立存在的，而是分布在某些平面结构上。

所以 Point-to-Plane ICP 通常比 Point-to-Point ICP 收敛更快，也更适合实际机器人场景。

---

3. NDT：不直接找点，而是把地图变成概率场

NDT，全称是 Normal Distributions Transform，正态分布变换。

它和 ICP 最大的区别是：

ICP 直接处理点，NDT 处理的是空间概率分布。

NDT 会先把目标点云地图划分成一个个 voxel 网格，然后对每个网格里的点建立一个高斯分布。

也就是说，NDT 不再关心某个具体点在哪里，而是关心：

这个区域里的点大概呈现什么分布？

当前扫描点云进来后，NDT 会不断调整位姿，让当前点云中的点尽可能落在目标地图中概率高的位置。

所以 NDT 更像是在问：

当前点云放在这个位置，像不像是从这张地图里扫描出来的？

这和 ICP 的思路明显不同。

ICP 是找最近邻。
NDT 是找概率最大的位置。

---

4. NDT 为什么适合 LiDAR 定位？

LiDAR 点云通常有几个特点：

• 点云比较稀疏；

• 远处点密度低；

• 扫描线分布不均匀；

• 当前帧和地图不可能完全一致；

• 环境里可能有动态物体；

• 噪声和遮挡很常见。

如果直接用 ICP 做最近邻匹配，这些因素都可能影响匹配稳定性。

而 NDT 把地图建成 voxel 里的高斯分布，不太依赖某一个具体点，所以在一些 LiDAR localization 场景里表现比较稳定。

尤其是在已有点云地图中做定位时，NDT 是一种比较常见的方案。

不过 NDT 也不是万能的。

它很依赖 voxel 分辨率。

voxel 太大，地图细节会被糊掉，定位精度下降。
voxel 太小，每个格子里的点太少，高斯分布不稳定，计算量也会上升。

这就很像调 ROS 参数：

调大了不行。
调小了也不行。
最后开始怀疑不是参数的问题，是自己的人生路径规划错了。

---

5. GICP：ICP 的加强版，开始考虑局部结构

GICP，全称是 Generalized ICP。

它可以理解为 ICP 的增强版本。

传统 ICP 主要关心点和点之间的距离，或者点到面的距离。

但 GICP 会进一步考虑每个点附近的局部几何结构。

它不只是问：

这个点和那个点近不近？

它还会问：

这个点附近像不像一个平面？
这个方向上的误差重不重要？
这个匹配在几何结构上合不合理？

GICP 会给点云中的点估计局部协方差矩阵，用协方差描述这个点附近的几何形状。

比如：

• 如果周围点主要分布在一个平面上，说明这是一个面结构；

• 如果周围点主要沿一条线分布，说明可能是边缘结构；

• 如果周围点分布很乱，说明这个地方的几何约束可能不稳定。

这样一来，GICP 的误差模型就比普通 ICP 更丰富。

它不是单纯地把点和点拉近，而是尽量让两个点云的局部结构也对齐。

---

6. GICP 为什么适合做精配准？

在实际点云配准中，GICP 经常用于精配准。

原因很简单：

它比普通 ICP 更懂点云的局部结构。

比如在 3D LiDAR 重定位里，我们可以先用一个全局粗配准方法得到大概位置，然后再用 GICP 做细化。

典型流程可以是：

全局粗配准
    ↓
得到一个大概初值
    ↓
GICP 精配准
    ↓
得到更准确的位姿

比如：

KISS-Matcher / Scan Context / 全局描述子
    ↓
GICP / small_gicp
    ↓
map → odom 位姿修正

这个流程比单独使用 ICP 或 GICP 更靠谱。

因为 GICP 虽然比 ICP 强，但它本质上仍然属于局部优化方法。

如果初值太离谱，它也可能会对齐失败。

所以做任意位置开机重定位时，不能指望 GICP 自己从全地图里把机器人找出来。

它更适合做：

粗配准之后的精修

这就像你不能让一个近视眼直接在人群里找人。
但你先告诉他“大概在左边第三排”，他就能发挥作用了。

---

7. 三者的核心区别

如果只记一个版本，可以这样理解：

ICP：点级别匹配

ICP 的核心是最近邻匹配。

它关心的是：

source 点经过变换后，离 target 点有多近？

它适合局部精配准，但非常依赖初值。

---

NDT：概率分布匹配

NDT 不直接找点对，而是把目标点云地图变成 voxel 高斯分布。

它关心的是：

当前点云落在地图概率高的位置了吗？

它适合 LiDAR 地图定位，对点云稀疏和噪声有一定鲁棒性。

---

GICP：带局部结构约束的 ICP

GICP 在 ICP 的基础上加入局部协方差。

它关心的是：

两个点不仅位置要接近，局部几何结构也要合理

它适合高精度精配准，但计算量更大，也需要较好的初值。

---

8. 工程里怎么选？

实际项目里，不应该简单问：

ICP、NDT、GICP 谁更强？

更应该问：

我有没有初值？
我要做粗配准还是精配准？
点云密度怎么样？
实时性要求高不高？
场景结构是否明显？

---

8.1 已经有较好初值

如果已经有比较好的初值，比如来自：

• 轮速里程计；

• IMU；

• LIO 里程计；

• 上一帧位姿；

• 粗配准结果；

那么可以用：

ICP / Point-to-Plane ICP / GICP

其中 GICP 通常精度更高，但计算更重。

---

8.2 做已有地图中的 LiDAR 定位

如果是机器人在已有点云地图中定位，可以考虑：

NDT / GICP

NDT 比较适合做 map-based localization。

GICP 更适合在已有初值基础上做局部位姿细化。

---

8.3 做任意位置开机重定位

如果机器人开机时完全不知道自己在哪里，不建议只靠 ICP 或 GICP。

因为它们本质上还是局部优化方法，需要一个比较靠谱的初始位姿。

更合理的方式是：

全局粗配准
    ↓
局部精配准
    ↓
持续跟踪定位

例如：

KISS-Matcher + GICP
Scan Context + ICP
全局描述子 + NDT

粗配准负责先把机器人从“完全迷路”拉回到“大概在这里”。
精配准负责再把位姿修到更准。

这个组合思路在实际系统里比单独押宝某一个算法更稳。

---

9. 常见踩坑

9.1 初值太差，ICP/GICP 直接跑飞

ICP 和 GICP 都不是全局搜索算法。

如果初值差得太远，它们很容易收敛到错误位置。

所以不要一看到配准失败就怀疑代码，也可能是你给它的初值太离谱。

算法不是神仙。
它只是优化器。

---

9.2 点云没有降采样，直接算到怀疑人生

点云配准通常需要 voxel downsample。

如果直接拿原始高密度点云去配准，计算量会非常大。

尤其是 GICP，本身就要估计局部协方差，如果不降采样，实时性很容易崩。

典型处理流程一般是：

原始点云
    ↓
去除无效点
    ↓
范围裁剪
    ↓
体素降采样
    ↓
配准

---

9.3 voxel 参数乱调

NDT、GICP、ICP 里经常都会涉及 voxel size、neighbor num、max correspondence distance 这类参数。

这些参数不是越小越好。

voxel 太小，计算量大，噪声敏感。
voxel 太大，细节丢失，精度下降。

这也是点云配准很像玄学的地方。

不是因为它真的玄学，而是因为参数、场景、点云质量、初值全都在一起影响结果。

---

9.4 地图和当前点云差异太大

如果建图时的环境和定位时的环境差异很大，比如：

• 多了很多障碍物；

• 少了很多结构；

• 有大量动态物体；

• 地图本身有畸变；

• 当前点云被遮挡严重；

那么配准效果都会受影响。

配准算法不是 PS 修图软件。
地图不对，点云不对，它也没法凭空变出正确位姿。

---

10. 总结

ICP、NDT、GICP 都是点云配准方法，但它们的思路不同。

ICP 最直观，核心是最近邻匹配。
NDT 更像概率地图匹配，适合 LiDAR 定位。
GICP 在 ICP 基础上加入局部结构约束，更适合高精度细配准。

在真实机器人系统里，它们通常不是单独使用，而是组合起来用。

比如做 3D LiDAR 重定位时，可以采用：

全局粗配准
    ↓
GICP / NDT / ICP 精配准
    ↓
持续发布 map → odom

这样比直接拿 ICP 或 GICP 硬怼全局重定位靠谱得多。

所以，不要简单问：

ICP、NDT、GICP 哪个最强？

更应该问：

我现在是要粗定位，还是精配准？
我有没有初值？
我的点云质量怎么样？
我的实时性要求高不高？

刚开始我也没想这么多。

我只知道 RViz 里的机器人一会儿飞墙，一会儿原地抽搐，一会儿地图和当前点云像分手多年互不相认。

后来才慢慢明白：

不是点云配准玄学。
是我一开始把所有算法都当成了同一把锤子。

ICP、NDT、GICP 都能锤。
但锤的钉子不一样。

选对了，是点云配准。
选错了，是 RViz 行为艺术。