4、匹配算法基础：最近邻匹配、ICP算法、NDT算法、相关性匹配
各位同学，欢迎来到第四章。前面我们聊了毫米波雷达的特性、点云怎么生成，也讲了地图的构建思路。那现在问题来了——你手里有一堆实时扫描的点，地图上也有参考点，怎么把它们对上？这就是匹配算法要干的事。
我入行那会儿，第一次做雷达匹配，天真地以为直接算个平移旋转就完事了。结果呢？点云一塌糊涂，匹配结果惨不忍睹。后来才明白，匹配算法选不对，后面做啥都是白搭。今天我就把这四种最经典的算法掰开揉碎了讲给你听。
4.1 最近邻匹配——最朴素但最实用
最近邻匹配，说白了就是找“离我最近的那个点”。你有一个实时点，去地图里找距离最近的点，认为它们是一对匹配。
听起来简单吧？但坑不少。
核心思想：对于每个源点云中的点，在目标点云中找到欧氏距离最近的点，建立对应关系。
我习惯用KD树来加速这个搜索过程。为啥？因为暴力搜索太慢了。假设你有10万个地图点，每个实时点都要算10万次距离，那计算量你想想看。
// 伪代码：最近邻匹配
for each point p in source_cloud:
nearest = find_nearest_in_target(p, target_cloud)
if distance(p, nearest) < threshold:
add_correspondence(p, nearest)这里有个关键参数——距离阈值。设太大，会引入错误匹配；设太小，又可能漏掉真匹配。我在项目中遇到过这种情况：阈值设成0.5米，结果动态障碍物和静态地图点匹配上了，定位直接飘了。后来我改成自适应阈值，根据点云密度动态调整，效果好了不少。
注意：最近邻匹配只考虑距离，不考虑几何结构。如果点云有重复结构（比如隧道、长廊），很容易匹配错。
4.2 ICP算法——迭代最近点的经典之作
ICP，全称Iterative Closest Point，是点云匹配界的“老大哥”。从1992年提出到现在，依然是很多系统的标配。
它的逻辑很直观：先找最近邻对应，然后算一个刚体变换（旋转+平移），把源点云变换一下，再重新找对应，再算变换……直到收敛。
嗯，这里要注意：ICP对初始位置很敏感。你初始位姿差太远，它就会掉进局部最优。我记得有一次做停车场定位，初始位置差了5米，ICP直接收敛到墙上去了。
// ICP算法核心步骤
while not converged:
// 步骤1：建立对应关系
correspondences = find_nearest_neighbors(source, target)
// 步骤2：计算最优变换
R, t = solve_svd(correspondences)
// 步骤3：应用变换
source = R * source + t
// 步骤4：检查收敛
if delta_error < epsilon:
breakICP的变种很多。点对点ICP、点对面ICP、广义ICP……我个人建议，如果点云质量好、噪声小，用点对点就够了。如果点云比较稀疏或者噪声大，试试点对面，它对平面结构更鲁棒。
经验之谈：ICP的收敛速度取决于初始位姿。我一般先用IMU或者轮式里程计给个初值，再用ICP精匹配。这样又快又稳。
4.3 NDT算法——正态分布变换的优雅解法
NDT，全称Normal Distributions Transform。它跟ICP的思路完全不同。ICP是在点层面做匹配，NDT是在“格子”层面做匹配。
怎么理解呢？你把地图点云划分成一个个小格子（voxel），每个格子里的点用一个高斯分布来描述。然后你找一个变换，让实时点云落在这些高斯分布上的概率最大。
说白了，NDT把匹配问题变成了一个概率最大化问题。
NDT的优势：不需要显式地找点对点对应关系，计算效率高，对初始位姿的容忍度比ICP好。
// NDT算法核心
// 步骤1：构建NDT地图
for each voxel in target_cloud:
mean = compute_mean(points_in_voxel)
covariance = compute_covariance(points_in_voxel)
// 步骤2：优化变换
function score(R, t):
total_score = 0
for each point p in source_cloud:
transformed_p = R * p + t
voxel = get_voxel(transformed_p)
score = gaussian_pdf(transformed_p, voxel.mean, voxel.cov)
total_score += score
return total_scoreNDT有个参数很关键——格子大小。格子太大，丢失细节；格子太小，计算量大且容易过拟合。我一般设成1米到2米之间，具体看雷达的角分辨率和场景特点。
我曾经在隧道场景里对比过ICP和NDT。隧道里结构单一，ICP经常匹配失败，但NDT因为用了概率分布，反而能稳定工作。所以，场景决定算法选择。
4.4 相关性匹配——基于图像思维的另类解法
相关性匹配，这个名字你可能在图像处理里听过。没错，它就是把点云投影成图像，然后在图像空间做匹配。
具体怎么做？你把雷达点云投影到二维栅格上，每个格子存一个值（比如高度、强度、占据概率）。然后你就有了两张“图”——实时图和地图图。接下来，用归一化互相关（NCC）或者相位相关来找最佳匹配位置。
// 相关性匹配流程
// 步骤1：投影点云到栅格
grid_map = project_to_grid(target_cloud, resolution)
grid_scan = project_to_grid(source_cloud, resolution)
// 步骤2：计算互相关
correlation = cross_correlate(grid_scan, grid_map)
// 步骤3：找到峰值位置
dx, dy = find_peak(correlation)相关性匹配的好处是：它对噪声不敏感，而且能处理大范围的初始偏差。但缺点也很明显——它只能做平移匹配，旋转得另外处理。我一般用FFT加速相关计算，不然实时性跟不上。
实用技巧：如果你用相关性匹配做粗匹配，再用ICP做精匹配，效果往往比单一算法好。这叫“粗到精”策略，我在多个项目里验证过。
4.5 四种算法对比与选型建议
讲了这么多，你可能想问：到底该用哪个？别急，我整理了一张表，帮你快速决策。
| 算法 | 核心思想 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|---|
| 最近邻匹配 | 找最近点建立对应 | 简单、快速 | 对噪声敏感、无几何约束 | 粗匹配、初始化 |
| ICP | 迭代优化刚体变换 | 精度高、理论成熟 | 对初值敏感、计算量大 | 精匹配、室内场景 |
| NDT | 概率分布匹配 | 效率高、鲁棒性好 | 格子参数敏感 | 室外大场景、隧道 |
| 相关性匹配 | 图像空间相关 | 抗噪声、大范围 | 仅平移、分辨率受限 | 粗匹配、2D场景 |
我个人习惯是：先用相关性匹配或者最近邻做粗匹配，把位姿拉到误差1米以内，然后用NDT或者ICP做精匹配。如果计算资源紧张，优先选NDT。如果精度要求极高，选ICP加点对面。
你想想看，匹配算法选对了，后面的定位和建图就顺了。选错了，后面再怎么调参数也是白费力气。
核心总结：没有万能的匹配算法。理解每种算法的假设和局限，根据你的传感器、场景和算力来选，这才是工程师该做的事。
这张图把四种算法的关系梳理清楚了。从输入点云开始，根据场景和需求选择算法，最终输出刚体变换。你可以把它当作选型参考图，贴在工位上。
好了，匹配算法的基础就讲到这里。四种算法各有千秋，没有绝对的好坏。关键是你得理解它们的原理，知道什么时候该用哪个。下一章我们会深入讲ICP的数学推导和代码实现，到时候手把手带你写一个ICP匹配器。