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) 来描述点云的局部分布

NDT算法公式推导及C++源码解析一_第1张图片

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

NDT算法公式推导及C++源码解析一_第2张图片

6.2 NDT scan registration

NDT算法公式推导及C++源码解析一_第3张图片

NDT算法公式推导及C++源码解析一_第4张图片

NDT算法公式推导及C++源码解析一_第5张图片

NDT算法公式推导及C++源码解析一_第6张图片

NDT算法公式推导及C++源码解析一_第7张图片

NDT算法公式推导及C++源码解析一_第8张图片

6.2.2 3D-NDT

NDT算法公式推导及C++源码解析一_第9张图片

NDT算法公式推导及C++源码解析一_第10张图片

NDT算法公式推导及C++源码解析一_第11张图片

NDT算法公式推导及C++源码解析一_第12张图片

NDT算法公式推导及C++源码解析一_第13张图片

简单的总结

NDT算法公式推导及C++源码解析一_第14张图片

NDT算法公式推导及C++源码解析一_第15张图片

NDT算法公式推导及C++源码解析一_第16张图片

 

你可能感兴趣的:(自动驾驶)