点云NDT算法（Normal Distribution Transform）是一种基于高斯分布的点云配准方法。与ICP算法相比，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算法

1. 将空间（reference scan）划分成各个格子cell
2. 将点云投票到各个格子
3. 计算格子的正态分布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} $$

4. 将第二幅scan的每个点按转移矩阵T的变换

5. 第二幅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) $$

6. 求所有点的最优值，目标函数为

$$
\Psi=\prod_{k=1}^np(T(\vec{p},\vec{x}_k))
$$

1 2	PDF可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息（朝向、平整度） 格子内少于3个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于5的cell，涉及到下采样方法。

• NDT的优化：

  格子参数最重要，太大导致精度不高，太小导致内存过高，并且只有两幅图像相差不大的情况才能匹配

参考资料

• https://blog.csdn.net/u013019296/article/details/129458116
• https://ghx0x0.github.io/2014/12/30/NDT-match/
• https://www.cnblogs.com/21207-iHome/p/8039741.html

文章链接：
https://www.zywvvd.com/notes/3d/registration/ndt/