一、NDT算法详解：从原理到实战

各位做激光定位的朋友，大家好。今天我们来聊聊NDT——正态分布变换。说实话，我刚入行那会儿，团队里还在用ICP做点云配准，后来转到NDT，感觉就像从手动挡换到了自动挡。不是说ICP不好，而是NDT在某些场景下，真的省心太多。

这一章，我会从原理讲起，再对比ICP，最后落到PCL的代码实现和自动驾驶中的实际应用。嗯，内容有点多，但都是干货。

1.1 正态分布变换（NDT）原理

NDT的核心思想，说白了就是：不直接匹配点，而是匹配概率分布。

你想想看，ICP的做法是找最近点，然后最小化距离。但点云是离散的，噪声一大，最近点就不靠谱了。NDT换了个思路——先把空间划分成网格，每个网格里统计点的分布，用高斯分布来描述。这样，一个网格就变成了一个概率模型。

具体怎么做呢？

第一步：把参考点云划分成固定大小的体素（voxel）。
第二步：每个体素内，计算所有点的均值 μ 和协方差 Σ。
第三步：用这个高斯分布 N(μ, Σ) 来表示该体素内的点云分布。

配准时，我们把当前帧的点云投影到参考坐标系下，然后计算每个点落在对应体素中的概率。所有点的概率加起来，就是我们要优化的目标函数。

数学上，NDT的得分函数是：

score = Σ exp( -0.5 * (x - μ)^T * Σ^(-1) * (x - μ) )

我们要做的，就是找到一组变换参数（平移+旋转），让这个得分最大。

这里有个细节——协方差矩阵 Σ 如果奇异怎么办？我在项目中遇到过，体素里点太少或者点共面，Σ 不可逆。我的习惯是加一个小的正则项，比如 Σ += λI，保证数值稳定。

1.2 NDT与ICP的对比

很多新手问我：NDT和ICP到底选哪个？我的回答是：看场景。

我整理了一个对比表，你一看就明白：

对比项	ICP	NDT
匹配对象	点对点（最近邻）	点对分布（概率模型）
初值敏感度	高，容易陷入局部最优	低，对初值容忍度更高
计算效率	慢，需要建KD树找最近点	快，体素化后直接查表
噪声鲁棒性	差，离群点影响大	好，概率模型天然抗噪
内存占用	低，只存点云	中等，需存体素分布参数

我个人习惯是：室内结构化场景用ICP，室外大场景用NDT。为什么？因为室外点云稀疏、噪声大，ICP经常跑飞。我曾经在高速公路上试过ICP，结果配准结果直接偏了5米——从那以后，室外场景我再也不敢只用ICP了。

避坑指南： 我曾经在隧道场景里用NDT，结果因为隧道两侧墙壁平行，NDT在纵向（隧道方向）的约束很弱，导致定位漂移。后来我加了轮速里程计做初值，才稳住。所以，NDT不是万能的，它需要好的初值。

1.3 PCL中NDT的实现与参数调优

PCL里NDT的实现非常成熟，我们直接看代码：

#include <pcl/registration/ndt.h>

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon(0.01);        // 收敛阈值
ndt.setStepSize(0.1);                      // 步长
ndt.setResolution(1.0);                    // 体素分辨率（米）
ndt.setMaximumIterations(35);              // 最大迭代次数

// 设置输入
ndt.setInputSource(source_cloud);
ndt.setInputTarget(target_cloud);

// 执行配准
pcl::PointCloud<pcl::PointXYZ> final_cloud;
ndt.align(final_cloud, initial_guess);

// 获取结果
Eigen::Matrix4f transform = ndt.getFinalTransformation();

参数调优是NDT的核心。我踩过不少坑，这里分享几个关键点：

分辨率（Resolution）：这是最重要的参数。太小，体素里点太少，分布不稳定；太大，精度下降。我的经验是：室内0.5-1.0米，室外1.0-2.0米。记得有一次在停车场，我把分辨率设成0.3米，结果每个体素里只有两三个点，协方差矩阵全是奇异的——嗯，教训深刻。
步长（Step Size）：牛顿法优化的步长。默认0.1米，如果场景变化剧烈，可以适当增大到0.5米。但别太大，否则会震荡。
最大迭代次数：一般30-50次就够了。我见过有人设200次，其实没必要，NDT收敛很快。

警告： 不要直接使用默认参数！PCL的默认分辨率是1.0米，但如果你用的是16线激光雷达，点云稀疏，1.0米可能不够。我建议先打印一下点云的密度，再决定分辨率。

1.4 NDT在自动驾驶定位中的应用

在自动驾驶中，NDT最常见的用法是点云地图匹配定位。流程是这样的：

离线构建高精度点云地图（通常是多次扫描拼接而成）。
在线运行时，当前帧激光点云与地图做NDT配准。
输出6自由度位姿（x, y, z, roll, pitch, yaw）。

这里有个关键点——地图的体素化。我们不会每次都重新计算地图的NDT分布，而是提前把地图体素化，存成NDT地图。这样在线运行时，只需要查表，速度飞快。

我参与过一个项目，在园区里做无人配送车。用的是32线激光雷达，地图大小约500m x 500m。我们把地图体素分辨率设为1.5米，预计算了所有体素的均值和协方差。在线运行时，NDT配准一次只需要20-30毫秒，完全满足实时性要求。

但要注意，动态物体会影响NDT。比如路上有行人、车辆，这些点不应该参与配准。我的做法是：先做地面分割，再过滤掉动态物体（比如用距离变化检测），剩下的静态点才送入NDT。

实战经验： 在自动驾驶中，NDT经常和IMU、轮速计做融合。NDT提供绝对位姿，IMU提供高频相对位姿，两者通过卡尔曼滤波结合。这样即使NDT偶尔失败（比如经过一片空旷区域），系统也不会立刻崩溃。

知识体系总览

下面这张图，是我画的本章节知识结构，方便你整体把握：

好了，NDT的核心内容就这些。从原理到对比，再到代码和实战，我希望你能感受到：NDT不是黑魔法，它只是一个更聪明的匹配方式。下次你在项目中遇到点云配准问题，不妨先试试NDT——说不定会有惊喜。

可可精品资料库

📚 课程目录

一、NDT算法详解：从原理到实战

1.1 正态分布变换（NDT）原理

1.2 NDT与ICP的对比

1.3 PCL中NDT的实现与参数调优

1.4 NDT在自动驾驶定位中的应用

知识体系总览