3. 匹配算法概述

好，咱们进入正题。前面聊完了雷达数据和高精地图各自长什么样，现在该让它们「对上眼」了。说白了，匹配算法就是解决一个问题：怎么把雷达扫到的点云，准确地放到地图的坐标系里。

我个人习惯把这件事比作「拼图」。你手里有一块碎片（雷达点云），想把它放到大拼图（高精地图）的正确位置上。怎么放？靠的就是匹配算法。今天咱们重点聊两个最经典的：ICP 和 NDT。

3.1 点云配准到底在干什么？

先别急着看公式。你想想看，配准的核心目标就一个：求出一个刚体变换矩阵。这个矩阵包含旋转和平移，能把雷达点云从「雷达坐标系」搬到「地图坐标系」。

数学上表达很简单：

P_map = R * P_radar + t

其中 R 是旋转矩阵，t 是平移向量。我们要找的就是最优的 R 和 t。

嗯，这里要注意：这个变换是刚体的。什么意思？就是点云里的点不会变形，不会拉伸，只是整体挪了个位置、转了个方向。我在项目中遇到过有人把缩放参数也加进去优化，结果地图匹配得一塌糊涂——因为雷达和地图之间不存在缩放关系。

3.2 ICP 算法：最经典的迭代最近点

ICP，全称 Iterative Closest Point。这玩意儿是点云配准的「老祖宗」，1980 年代就有人提出来了。到现在很多高级算法里，依然能看到它的影子。

ICP 的思路非常直观：

1. 对于雷达点云中的每个点，在地图点云中找到最近的那个点。
2. 根据这些「配对」的点对，计算出一个最优的变换。
3. 用这个变换更新雷达点云的位置。
4. 重复 1-3 步，直到收敛。

说白了就是「你离我最近，咱俩就是一对，然后我往你那边挪一挪」。听起来挺朴素的，对吧？

我曾经在一个停车场项目里用过 ICP。当时环境很干净，墙面平整，ICP 跑得飞快，精度也够用。但后来换到有大量灌木丛的场景，ICP 就开始「犯浑」了——因为灌木丛的点云分布太散，最近点匹配经常找错对象。

ICP 的优缺点：

优点	缺点
原理简单，容易实现	对初始位姿敏感
收敛速度快（在好的初始值下）	容易陷入局部最优
精度较高	对噪声和离群点敏感
不需要提取特征	计算量大（需要搜索最近点）

3.3 NDT 算法：更鲁棒的正态分布变换

NDT，全称 Normal Distributions Transform。这玩意儿是 ICP 的「升级版」，或者说「换个思路的版本」。

ICP 是点对点的匹配，NDT 是点对面（或者说点对分布）的匹配。它先把地图点云划分成一个个小网格（voxel），每个网格里计算点云的正态分布（均值和协方差）。然后，把雷达点云的点放到这些网格里，计算这个点落在当前网格分布中的概率。

你想想看，这样有什么好处？

• 不依赖最近点搜索，所以计算效率更高。
• 对初始位姿不那么敏感，因为网格里的分布是平滑的。
• 天然抗噪声，因为正态分布本身就在描述点的统计特性。

我在高速公路上做定位时，特别喜欢用 NDT。高速场景变化大，车流多，点云噪声也大。NDT 在这种环境下比 ICP 稳定得多。有一次在隧道里，GPS 信号全丢，纯靠 NDT 匹配高精地图，跑了 3 公里误差没超过 20 厘米。

NDT 的优缺点：

优点	缺点
对初始位姿容忍度更高	网格大小需要调参
计算效率高（尤其点云量大时）	精度略低于 ICP（在理想条件下）
抗噪声能力强	对网格划分方式敏感
适合大规模场景	实现比 ICP 复杂一些

3.4 ICP vs NDT：怎么选？

很多同学问我：「老师，到底用 ICP 还是 NDT？」

我的回答是：看场景。

• 场景干净、初始位姿准、要求高精度 → 选 ICP。比如室内停车场、封闭园区。
• 场景复杂、噪声多、初始位姿不确定 → 选 NDT。比如高速公路、城市道路。
• 实时性要求高 → 优先考虑 NDT，因为它可以用多线程加速网格计算。

当然，现在很多工程方案是两者结合：先用 NDT 粗匹配，把位姿拉到比较准的范围，再用 ICP 精匹配，榨出最后那点精度。我在量产项目里就是这么干的，效果很稳。

3.5 本章知识体系

下面这张图总结了本章的核心逻辑，我手绘了一个 SVG，方便你理解：

这张图把整个流程串起来了：左边是雷达点云，右边是高精地图，中间经过 ICP 或 NDT 算法，最终输出一个刚体变换矩阵。你把这个矩阵套到雷达点云上，点云就「对齐」到地图上了。

3.6 小结

这一章咱们聊了匹配算法的核心概念。ICP 和 NDT 是两大基石，各有各的脾气。我个人建议：先理解 ICP 的原理，再学 NDT 的改进思路。因为很多 NDT 的变种算法，本质上还是在解决 ICP 的痛点。

下一章咱们会深入代码，手撸一个 ICP 的实现。到时候你会发现，理论懂了，写代码其实没那么难。