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

NDT的具体原理和步骤:

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

误差函数的构建:

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

优化过程及注意事项:

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

点云匹配算法NDT公式推导过程说明,总结_第1张图片

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

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

点云匹配算法NDT公式推导过程说明,总结_第2张图片

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

点云匹配算法NDT公式推导过程说明,总结_第3张图片

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

点云匹配算法NDT公式推导过程说明,总结_第4张图片

 总结:

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

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

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

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

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

 

你可能感兴趣的:(多传感器融合课,激光slam,slam,自动驾驶,点云)