NDT算法公式推导及C++源码解析一

最近在研究无人车定位技术中的NDT算法的CUDA加速，所以首先还是要理解算法原理和CPU实现。这个系列总共两篇，是目前找到的比较详实的文章。
版权声明：本文为CSDN博主「jyakaranda」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/u013794793/article/details/89306901

NDT 的论文在去年就看了，代码的话也零零散散看了一些，但直到最近才决定抽出时间把 NDT 的论文和代码重新整理记录一下，方便日后学习。本文主要参考的论文有 [1,2,3]，其中 [1] 是 NDT 被首次提出时发表的论文，如论文名所指出的——NDT 是一种 laser scan matching 方法。[2] 则是 Magnusson 等人将 NDT 从 2D 扩展到 3D 中的论文，[3] 是 Magnusson 的博士论文，论文的内容很多，我阅读的主要是介绍 NDT 的第六章，内容十分清晰详实。[4,5] 则是在 [3] 中的参考文献找到的论文，挑着 [4] 的高斯近似看了一下。

简单介绍
NDT 的直观介绍的话建议看 [1] 或 [7]，这里就不重复赘述了，本文主要关注的还是 NDT 的公式推导（不含 line search 部分的公式）和源码解析（不含 line search 部分的代码），公式主要来自于 [3] 第六章，代码主要来自于 Autoware[6] 的 ndt_cpu 库。

6.1 NDT for representing surfaces
直接使用点云的缺点是：1. 点云中没有直接包含平面的特征信息，如朝向、平滑性等；2. 直接使用点云有点 inefficient，需要大量存储空间。NDT 则使用局部 PDF(probability density function) 来描述点云的局部分布

其好处是：1. 正态分布局部是平滑的，具有连续的导数；2. 每个 PDF 可以认为是局部平面的近似，描述了平面的位置（均值 μ\muμ）、朝向、形状和平滑性（协方差 Σ\SigmaΣ 的特征向量和特征值，特征向量描述的点云分布的主成分（principal components））等特征。在 2D 场景中，1. 如果方差比较近似，局部点云的形状为 point；2. 如果一个方差远大于另一个方差，局部点云的形状为 line。在3D 场景中，1. 如果方差比较近似，局部点云形状为 point/sphere；2. 如果一个方差远大于另外两个方差，局部点云形状为 line；如果一个方差远小于另外两个方差，局部点云形状为 plane。如图 6.4。

6.2 NDT scan registration

6.2.2 3D-NDT

简单的总结