4. 激光SLAM前端-帧间匹配：ICP、PL-ICP、NDT 与 PCL 实践
帧间匹配，说白了就是让机器人知道「上一帧和这一帧之间我动了多少」。激光SLAM里，这事通常交给点云配准来做。我最早接触这玩意儿时，觉得不就是把两坨点云对上嘛，后来才发现里面门道不少。今天咱们就把 ICP、PL-ICP、NDT 这几个核心算法捋一遍，再聊聊怎么用 PCL 快速实现。
4.1 ICP 算法：最朴素的点云对齐
ICP（Iterative Closest Point）的思路特别直接——假设你有两帧点云，源点云和目标点云。每次迭代，找源点云中每个点在目标点云中的最近邻，然后计算一个旋转平移让这些点对的距离平方和最小。嗯，说白了就是反复做两件事：找对应，求变换。
数学上，我们最小化这个目标函数：
E(R,t) = Σ || R·p_i + t - q_i ||²其中 p_i 是源点云中的点，q_i 是它在目标点云中的最近邻。求解 R,t 可以用 SVD 分解，也可以用四元数。我个人习惯用 SVD，因为数值稳定，代码也好写。
- 对源点云每个点，在目标点云中找最近邻（KD-Tree加速）
- 计算去质心坐标，构造协方差矩阵
- SVD分解得到旋转矩阵 R，再计算平移 t
- 判断收敛：若 ΔR,Δt 小于阈值或迭代次数超限则停止
我在项目里遇到过一个问题：ICP 对初始位姿非常敏感。有一次在长廊环境，机器人稍微转快点，帧间匹配就直接飞了。后来我加了 IMU 做初值，才稳住。所以记住，ICP 不是万能的，初值不好它就给你乱来。
4.2 PL-ICP：从点到点，进化到点到线
PL-ICP（Point-to-Line ICP）是 ICP 的一个强力变种。它不再用点对点的距离，而是计算源点到目标点云局部拟合直线的距离。你想想看，在结构化环境里（比如室内墙壁、走廊），点到线的约束比点到点更符合物理世界——因为激光点本来就落在平面上。
目标函数变成：
E(R,t) = Σ ( n_j^T · (R·p_i + t - q_j) )²其中 n_j 是目标点云中 q_j 处的法向量。这个改动让收敛精度明显提升，尤其在人造场景中。我记得在某个仓库项目里，用普通 ICP 匹配出来的轨迹总有点飘，换成 PL-ICP 之后，建图质量肉眼可见地变好了。
4.3 NDT：换个思路，用概率说话
NDT（Normal Distributions Transform）和 ICP 家族完全不同。它把目标点云划分成体素网格，每个体素里用高斯分布描述点的分布。匹配时，我们最大化源点云落在目标点云概率密度场中的总概率。说白了，就是让源点云在目标点云的“地形图”上找到最舒服的位置。
NDT 的好处很明显：不用显式找对应点，速度比 ICP 快不少，而且对初值没那么敏感。我一般在处理大场景（比如室外园区）时优先用 NDT，体素分辨率设 1~2 米，效果很稳。
| 算法 | 核心思想 | 收敛速度 | 精度 | 对初值敏感度 |
|---|---|---|---|---|
| ICP | 点对点距离 | 慢 | 中等 | 高 |
| PL-ICP | 点到线距离 | 中等 | 较高 | 中等 |
| NDT | 正态分布概率 | 快 | 高（调参后） | 低 |
4.4 PCL 库实现点云配准
PCL（Point Cloud Library）里把这些算法都封装好了。我直接给你看一段常用的 ICP 配准代码，这是我在实际 ROS 节点里用过的骨架：
#include <pcl/registration/icp.h>
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source_cloud);
icp.setInputTarget(target_cloud);
icp.setMaxCorrespondenceDistance(0.3); // 30cm
icp.setMaximumIterations(50);
icp.setTransformationEpsilon(1e-8);
icp.setEuclideanFitnessEpsilon(0.01);
pcl::PointCloud<pcl::PointXYZ> aligned;
icp.align(aligned);
Eigen::Matrix4f T = icp.getFinalTransformation();换成 PL-ICP 的话，PCL 里没有直接封装，但可以用 pcl::IterativeClosestPointWithNormals 或者自己实现法向量约束。NDT 则用 pcl::NormalDistributionsTransform，接口类似。
最后说一句，PCL 的配准模块还提供了 pcl::registration::TransformationEstimationSVD 等底层工具，如果你想自己写更灵活的匹配器，可以直接用这些组件。嗯，帧间匹配是 SLAM 前端的基石，选对算法、调好参数，后面的建图才能稳。