51工具盒子点云NDT算法（Normal Distribution Transform）是一种基于高斯分布的点云配准方法。与ICP算法相比，NDT算法更适用于不规则形状、噪声和部分遮挡的场景，本文记录相关内容。
NDT {#NDT}
将点云转换为高斯分布的函数形式，通过计算不同高斯分布函数之间的匹配来进行点云配准。具体来说，算法首先将一个点云转换为三维高斯分布函数（即NDT分布），然后通过最小化两个点云之间的NDT分布函数之间的KL散度来进行点云配准。
具体来说，NDT算法首先将原始点云数据离散化为一个三维的网格（voxel grid），并对每个网格中的点云进行采样和特征提取。这里采用了点云法向量作为特征，通过计算每个网格中点云的均值和协方差矩阵，可以得到一个高斯分布，即一个GMM。NDT算法会将目标点云和参考点云都转化成GMM的形式，然后计算两个GMM之间的KL散度，用于评估它们之间的相似度。在计算KL散度时，NDT算法采用了一种高效的方法，即通过将每个网格中心点的高斯分布变换到参考点云坐标系下，可以大大减少计算复杂度。
最后，NDT算法通过最小化两个GMM之间的KL散度来计算相对位姿变换，得到一个最优的刚体变换矩阵，从而实现点云配准。由于NDT算法采用了高效的计算方法，因此在处理大规模点云数据时具有较高的效率和精度。
简要步骤 {#简要步骤}
主要分为两步：NDT建图和NDT匹配。
-
在NDT建图阶段，算法将一个点云转换为高斯分布函数，并将其存储为一个栅格地图。
-
在NDT匹配阶段，算法将两个点云都转换为高斯分布函数，并使用最小化KL散度的方法来找到它们之间的最佳匹配。
与ICP算法相比，NDT算法具有更高的配准精度和鲁棒性，尤其是在噪声和不规则形状的情况下。然而，NDT算法的计算量相对较大，需要较长的处理时间。
NDT算法 {#NDT算法}
- 将空间（reference scan）划分成各个格子cell
- 将点云投票到各个格子
- 计算格子的正态分布PDF参数
$$ \begin{aligned}&\vec{\mu}=\frac{1}{m}\sum_{k=1}^{m}\vec{y}{k},\\&\Sigma=\frac{1}{m-1}\sum{k=1}^{m}(\vec{y}{k}-\vec{\mu})(\vec{y}{k}-\vec{\mu})^{\mathrm{T}},\end{aligned} $$
-
将第二幅scan的每个点按转移矩阵T的变换
-
第二幅scan的点落于reference的哪个格子，计算响应的概率分布函数
$$ p(\vec x)=\frac{1}{(2\pi)^{D/2}\sqrt{|\Sigma|}}\exp\left(-\frac{(\vec x-\vec\mu)^{\mathrm{T}}\Sigma^{-1}(\vec x-\vec\mu)}{2}\right) $$
- 求所有点的最优值，目标函数为
$$
\Psi=\prod_{k=1}^np(T(\vec{p},\vec{x}_k))
$$
-
NDT的优化：
格子参数最重要，太大导致精度不高，太小导致内存过高，并且只有两幅图像相差不大的情况才能匹配
参考资料 {#参考资料}
- https://blog.csdn.net/u013019296/article/details/129458116
- https://ghx0x0.github.io/2014/12/30/NDT-match/
