点云匹配算法NDT公式推导过程说明，总结

NDT的具体原理和步骤：

https://blog.csdn.net/qq_35102059/article/details/121694705?spm=1001.2014.3001.5501

误差函数的构建：

将点云Y投影到点云X坐标系下，迭代计算最大重合概论。

优化过程及注意事项：

1.计算点云x的均值和方差，根据预测（可以使imu给的等）的位姿将Y投影到X，再对数化，将问题转化为求目标函数的最小值。

2.下图为借助泰勒展开，化简最小化问题，需要注意的是R，t和p均表示位姿转换。

3.推导过程中，b为1*1的矩阵等于自身的转置，然后合并，b在后面也一样转换了一次

 4.对变化量求导，令其为0，即可求得最小时的△p，即位姿迭代值。

5.最后分别为2d和3d的一些直接可代入的结果

 总结：

1.NDT就是一个投影算概论，再最小二乘优化的问题，推导难度不如ICP。

2.最小二乘优化问题有高斯牛顿、LM、牛顿法、梯度下降法、狗腿法等。

3.NDT不需要多少的改进就可以得到较好的计算结果，比ICP起点高。

4.NDT可以对点云做一些降采样、栅格化等处理，降低计算复杂度。

5.单从整个匹配过程看，NDT和ICP差不太多。但NDT可以在同一位置附近都使用同一帧点云（类似于选取关键帧）计算的均值和方差，避免每一帧都选取。而ICP每一帧都需要做投影关联。