4. 局部重对齐：利用ICP或NDT将漂移后的点云/位姿重新对齐到局部地图

好，咱们接着聊。上一节我们搞定了全局重对齐，说白了就是让机器人知道自己大概在哪儿。但全局重对齐有个问题——它太慢了，而且对初始位姿要求高。你想想看，如果机器人只是稍微漂了一点，你非得把它拉到全局地图里重新匹配一遍，那计算量可就大了去了。

所以，实际工程里我更常用的是局部重对齐。它的思路很简单：既然漂移量不大，那我就在当前位姿附近，用局部地图做一次精细的配准。嗯，这里要注意，局部重对齐的核心是两个算法——ICP和NDT。我分别讲讲我的使用心得。

4.1 ICP：最经典的点对点配准

ICP，全称Iterative Closest Point，迭代最近点。它的原理说白了就是：把两片点云叠在一起，找最近的点对，然后算一个旋转平移矩阵，让这些点对的距离最小。然后重复这个过程，直到收敛。

我在项目中遇到过一个问题：ICP对初始位姿非常敏感。如果初始偏差太大，它很容易陷入局部最优。所以我的习惯是，先用里程计或者IMU给一个差不多的初值，再用ICP做精细调整。

下面是一个简单的ICP实现示例，我用的是PCL库：

// 输入：源点云source，目标点云target，初始位姿init_guess
// 输出：优化后的变换矩阵final_transform

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source);
icp.setInputTarget(target);
icp.setMaxCorrespondenceDistance(0.5);  // 最大对应距离
icp.setMaximumIterations(50);           // 最大迭代次数
icp.setTransformationEpsilon(1e-8);     // 收敛阈值
icp.setEuclideanFitnessEpsilon(1e-6);   // 均方误差阈值

pcl::PointCloud<pcl::PointXYZ> aligned;
icp.align(aligned, init_guess);

if (icp.hasConverged()) {
    final_transform = icp.getFinalTransformation();
    std::cout << "ICP收敛，得分：" << icp.getFitnessScore() << std::endl;
} else {
    std::cerr << "ICP未收敛，请检查初始位姿" << std::endl;
}

4.2 NDT：更鲁棒的分布配准

NDT，Normal Distributions Transform，正态分布变换。它和ICP的思路不太一样。ICP是点对点，NDT是把点云划分成网格，每个网格里统计点的正态分布。然后配准时，不是找最近点，而是最大化源点云在目标点云网格中的概率密度。

说白了，NDT更「软」一些。它对噪声和局部形变的容忍度更高。我记得有一次做室外场景的定位，点云里有很多树叶和灌木，ICP死活配不准，换成NDT一下就搞定了。

NDT的代码实现也很直接：

// 输入：源点云source，目标点云target，初始位姿init_guess
// 输出：优化后的变换矩阵final_transform

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setInputSource(source);
ndt.setInputTarget(target);
ndt.setResolution(1.0);              // 网格分辨率，单位米
ndt.setStepSize(0.1);                // 步长
ndt.setTransformationEpsilon(1e-8);  // 收敛阈值
ndt.setMaximumIterations(35);        // 最大迭代次数

pcl::PointCloud<pcl::PointXYZ> aligned;
ndt.align(aligned, init_guess);

if (ndt.hasConverged()) {
    final_transform = ndt.getFinalTransformation();
    std::cout << "NDT收敛，得分：" << ndt.getFitnessScore() << std::endl;
} else {
    std::cerr << "NDT未收敛，请调整参数" << std::endl;
}

4.3 ICP vs NDT：怎么选？

这个问题我经常被问到。我的回答是：没有绝对的好坏，只有合不合适。下面这张表是我个人经验总结的：

我个人习惯是：室内用ICP，室外用NDT。但也不是绝对的。如果室内场景点云质量不好，我也会切到NDT试试。你想想看，工程上最重要的是稳定，不是炫技。

4.4 局部重对齐的完整流程

好了，理论讲完了，咱们看看实际怎么用。下面这张图是我自己画的局部重对齐流程图，你可以参考一下：

流程其实不复杂。拿到当前点云后，先做降采样，减少计算量。然后根据场景选ICP还是NDT。配准完成后，检查收敛性和得分。如果得分太高，说明可能配错了，这时候我会回退到上一帧的位姿，或者触发全局重对齐。

好了，局部重对齐就讲到这里。核心就是记住：ICP适合精细调整，NDT适合鲁棒配准。具体用哪个，得看你的场景和需求。下一节我们会聊聊怎么把这些技术整合到一个完整的定位系统里。