📚 课程目录第4章初始配准算法

4. 初始配准算法

聊到激光雷达和高精地图的匹配，我脑子里第一个蹦出来的词就是「配准」。说白了，就是把雷达当前扫到的点云，跟地图里存好的点云对上。这事儿看着简单，做起来坑不少。今天咱们就把几种主流算法掰开揉碎聊一聊。

4.1 ICP算法：最朴素的迭代思路

ICP，全称Iterative Closest Point，迭代最近点。这玩意儿是点云配准的祖师爷。我最早接触它是在2016年，那时候做机器人定位，用的就是最原始的ICP。

核心思想其实就一句话：找到两组点云中距离最近的点对，然后算一个旋转平移矩阵，让这些点对的距离之和最小。然后重复这个过程，直到收敛。

嗯，这里要注意——它假设两组点云已经大致对齐了。如果初始位置差太远，它很容易掉进局部最优。

  ICP的数学本质： 最小化目标函数 E(R,t) = Σ||R·pi + t - qi||²，其中pi是源点云中的点，qi是目标点云中最近的点。

我在项目中遇到过一个问题：ICP对离群点特别敏感。如果雷达扫到了动态障碍物（比如一辆车开过去），这些点会被强行匹配到地图上，结果就是配准结果漂移。所以实际用的时候，我一般会先做个滤波，把太远的点扔掉。

代码实现其实不复杂，核心步骤就这几行：

def icp(source, target, max_iter=50):
    for i in range(max_iter):
        # 1. 找最近邻
        correspondences = find_nearest_neighbors(source, target)
        # 2. 计算变换矩阵
        R, t = solve_rigid_transform(source, target, correspondences)
        # 3. 应用变换
        source = R @ source + t
        # 4. 检查收敛
        if loss < threshold:
            break
    return R, t

你想想看，这个流程是不是很直观？但实际调参的时候，收敛阈值、最大迭代次数、最近邻搜索的半径，每个参数都得试。我建议你先用K-D树加速最近邻搜索，不然点云一多，速度慢得让人抓狂。

4.2 Point-to-Plane ICP：精度提升的关键

原始的Point-to-Point ICP有个毛病——它只考虑点与点之间的距离。但在结构化场景中（比如墙面、地面），点与点之间的法向量信息其实更有价值。

Point-to-Plane ICP改进了目标函数： 它不再最小化点到点的距离，而是最小化点到目标点所在平面的距离。

说白了，就是让源点云中的点，沿着目标点云的法向量方向去对齐。这样做的好处是，在平面区域收敛更快，精度也更高。

我的经验： 在停车场或者室内走廊这种场景，Point-to-Plane ICP比普通ICP好使得多。我曾经在一条长走廊里做定位，普通ICP迭代了80次还没收敛，换成Point-to-Plane，15次就搞定了。

目标函数变成了这样：

E(R,t) = Σ [(R·p<sub>i</sub> + t - q<sub>i</sub>) · n<sub>i</sub>]²

其中n_i是目标点q_i的法向量。这个改动看着小，但效果天差地别。

不过要注意——法向量的计算质量直接影响结果。如果点云噪声大，算出来的法向量是歪的，那配准结果也会歪。我一般会先做个平滑滤波，再用法向量估计。

4.3 NDT算法：概率视角的配准

NDT，Normal Distributions Transform，正态分布变换。这玩意儿跟ICP的思路完全不同。

ICP是在找点与点的对应关系，NDT是在找分布与分布的对应关系。

具体怎么做呢？先把目标点云划分成网格（voxel），每个网格里计算点云的高斯分布（均值和协方差）。然后，把源点云的点投影到这些网格里，计算每个点落在对应网格的概率。最大化所有点的概率之和，就是我们要的变换矩阵。

  NDT的优势： 不需要显式地找最近邻，所以对初始位置的要求没那么苛刻。而且它天然对离群点有鲁棒性，因为概率模型会自动降低异常点的权重。

我记得有一次做高速公路场景的配准，车在高速移动，点云变形严重。用ICP死活对不上，换成NDT反而一次就过了。原因就是NDT不依赖点对点的精确匹配，而是看整体分布是否吻合。

NDT有个关键参数——网格大小。网格太大，精度不够；网格太小，计算量爆炸，而且容易过拟合。我个人习惯先设个2米×2米的网格试试，效果不好再调。

# NDT伪代码
def ndt(source, target, voxel_size=2.0):
    # 1. 构建目标点云的NDT网格
    grid = build_ndt_grid(target, voxel_size)
    # 2. 迭代优化
    for i in range(max_iter):
        score = 0
        for point in source:
            # 找到点所在的网格
            cell = grid.locate(point)
            # 计算该点落在网格中的概率
            prob = gaussian_pdf(point, cell.mean, cell.cov)
            score += prob
        # 更新变换矩阵，最大化score
        R, t = optimize(score, source)
    return R, t

4.4 SAC-IA粗配准：给ICP一个好起点

前面说的ICP和NDT，本质上都是精配准——它们需要初始位置已经比较接近。但如果两个点云离得远，比如刚开机时雷达扫到的第一帧跟地图差了几十米，那直接上ICP肯定翻车。

这时候就需要粗配准了。SAC-IA（Sample Consensus Initial Alignment）就是干这个的。

它的思路很暴力：从源点云中随机选几个点，然后在目标点云中找特征相似的点对，算出一个变换矩阵。重复这个过程很多次，挑出效果最好的那个。

注意： SAC-IA依赖点云的特征描述子，最常用的是FPFH（Fast Point Feature Histogram）。如果场景太单调（比如一片白墙），特征区分度不够，SAC-IA也会失效。

我踩过这个坑。有一次在隧道里做实验，隧道壁全是混凝土，纹理几乎没有。SAC-IA随机采样了1000次，每次找到的对应点都是错的。后来我加了IMU的航向角信息，把搜索范围缩小了，才勉强跑通。

SAC-IA的流程大致如下：

计算源点云和目标点云的FPFH特征
随机采样n个点（一般3-5个）
在目标点云中找特征最相似的n个点
用这n对点计算变换矩阵
用该变换矩阵对齐点云，计算匹配得分
重复步骤2-5，直到达到最大迭代次数
返回得分最高的变换矩阵

你想想看，这个流程是不是有点像RANSAC？没错，它本质上就是RANSAC在点云配准上的应用。

4.5 四种算法的对比与选择

聊了这么多，到底该用哪个？我整理了一张表，方便你对照：

算法	适用场景	优点	缺点
Point-to-Point ICP	初始位置接近、点云质量好	实现简单、速度快	对离群点敏感、需要好初值
Point-to-Plane ICP	结构化场景（墙面、地面）	精度高、收敛快	依赖法向量质量
NDT	点云变形大、噪声多	鲁棒性好、不需要精确对应	网格参数难调、计算量大
SAC-IA	初始位置未知、需要粗配准	不依赖初值、全局搜索	依赖特征描述子、速度慢

实际项目中，我常用的组合是：先用SAC-IA做粗配准，把两个点云拉到差不多位置，然后用Point-to-Plane ICP或者NDT做精配准。这套组合拳在大多数场景下都够用。

避坑指南： 我曾经在雨雪天气做实验，点云里全是噪点。SAC-IA的FPFH特征完全失效，ICP也一直震荡。后来我把点云做了降采样，又加了体素滤波，才勉强稳住。所以预处理真的很重要，别一上来就怼算法。

4.6 本章知识体系

下面这张图总结了四种算法的核心逻辑和适用关系，你可以对照着理解：

嗯，四种算法各有各的脾气。没有银弹，只有合适的场景。你在实际项目中多试试，慢慢就能找到感觉了。