点云配准NDT（P2D）算法详解
点云配准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) 总体流程
