点云配准NDT (P2D)算法详解

最新推荐文章于 2026-03-24 00:51:12 发布

原创

最新推荐文章于 2026-03-24 00:51:12 发布 · 8.1k 阅读

标签

#算法

本文详细介绍了点云配准算法NDT，它利用正态分布对参考点云进行表示，通过计算均值向量和协方差矩阵实现局部表面拟合。重点讲解了正态分布的原理、特征向量、网格划分及优化过程，以及PCL实现的代码实例。

点云配准NDT (P2D)算法详解

最近了解了一些关于点云配准算法NDT的相关文章，进行总结一下。

NDT算法的关键是其利用正态分布对参考点云进行了重新表示，使用点云在一个模型特定位置的似然值而不是直接使用点云进行匹配。

先验知识（正态分布）

一维正态分布表达式：
在这里插入图片描述
D维正态分布表达式（本文中D为3）：

其中，均值向量μ和方差矩阵Σ通过下式计算：

在一维的情况，正交分布随机变量X有一个期望值μ，并且对于μ的不确定性用Σ来表示。上式中的多变量概率函数在D=1的情况下降低为一维。均值和方差通过一个均值向量μ和方差矩阵Σ表示。方差矩阵对角线元素代表每个变量的方差，非对角线元素代表变量之间的协方差。下图解释了一维、二维、三维正交分布的正交变换概率密度函数。
在这里插入图片描述特征向量描述了分布的主要成分，即，一组对应于变量协方差矩阵Σ的主方向的正交向量。而一个三维正交变换的不同形状，取决于协方差矩阵Σ特征值的关系。箭头展示了分布的特征向量方向，通过对应的特征值调整大小。
（即：三个特征值的大小接近为球或点，一个特征值远大于另外两个特征值为线状，两个特征值远大于一个特征值为平面）
在这里插入图片描述
正交变换可以被作为表面紧凑表示的一个方法。这一变换把点云投影为一个光滑曲面表示，描述为一个局部概率密度函数的集合，描述一部分曲面的形状。

如下图中所示，左侧为一个三维点云，右侧为对应的NDT表示：
在这里插入图片描述

整体过程

1) 建立NDT

把二维/三维点云在空间上划分为格网或立方体，将点云中的点归于对应的网格并统计每个cell中的点数，若点数 >5则有效（避免点数过小导致的共面或协方差矩阵奇异不能求逆的问题）。利用空间坐标计算均值向量μ和协方差矩阵Σ。（公式中的符号进行相应的变化）
在这里插入图片描述

则每一个网格中的概率密度函数为：

2) 目标函数

当使用NDT来进行配准时，其目标是找到当前扫描中点在参考扫描表面的最大似然。要进行优化的参数有：当前扫描位置估计的旋转和平移，可以被编码为向量P。当前扫描通过点云X表示。假设有一个空间变换函数 T(P,X) 能够在空间中通过p移动点x。对扫描点给定一些概率密度函数P(x),则最优的变换p应该为最大似然函数那一个。即最大化下式：
在这里插入图片描述
等效变换上述目标函数为 负log对数如下所示：

下图中，左侧（a）为正态分布（红色）与混合分布（正态、均匀分布，绿色）的图像，右侧（b）为两者相应的-log对数。由（b）中可知，原始的正态分布相对于X没有约束，在x的值变大时函数值也会受到很大影响，而混合模型则会对x的值进行约束，具有更好的鲁棒性。（受cell内噪声点的影响更小）。
在这里插入图片描述
因此，我们选择右侧（b）中的绿色曲线对应的函数作为优化的目标函数，如下所示：

式中，C1，C2为对应的常数，调整一个网格内的P(x)函数的积分值为1。p0是预期的噪声比例。如上图所示，使用此函数能够对噪声的影响进行约束。

因此，目标函数转换为如下形式：
在这里插入图片描述
但是，虽然由图中可以看出上式是连续的（能够进行求导），但是其并没有一个简单的一阶/二阶导数，所以可以通过转换为高斯函数的形式：

设置x分别为0，σ，∞时，通过下式表述 d1，d2，d3：

所以，一个扫描点在对应NDT score函数上的影响为：（去除了d3是因为其为一个常数，优化过程中可以忽略）。
在这里插入图片描述
给定一个点集X = {x1, x2, …，xn}，一个变换p，以及一个空间变换函数 T(P,X) 在空间中通过p变换X，则其对应的 score 函数为：

3) 牛顿法优化

上一节中我们建立了 score 函数，则现在问题变为找到一个最优的p，使得 score 函数值最小。文章中使用了传统的牛顿法来处理这一优化过程。即迭代的求解

这一函数。其中，H为海森矩阵，g为S§的梯度向量，每次迭代求解出的增量△p加到当前的转换向量P中，即：
在这里插入图片描述
梯度向量g中的元素为：

海森矩阵H中的元素为：

4) 总体流程

好了，上述基本就描述了NDT算法的实现步骤，其总体流程如下：
在这里插入图片描述

5) PCL实现

最后附上pcl实现代码（运行环境WIN 10 + PCL1.8.0 + VS2013），不知道为什么程序的效率有点低，大概需要30s（3000 ms）左右才能结束，而其他人都是以ms为时间单位的。大家感兴趣可以尝试跑一下。测试数据可以通过这里下载。

/*
NDT算法点云配准
xiaochen wang
2021/06
*/
#include <iostream>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <time.h>

using namespace std;

pcl::PointCloud<pcl::PointXYZ>::Ptr read_point_clouds(string const &file_path);