NDT算法公式推導及C++源碼解析一

原創

2020-06-27 19:35

最近在研究無人車定位技術中的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。