點雲NDT配準方法介紹

三維配準中常常被說起的配准算法是ICP迭代的方法，這種方法通常般須要提供一個較好的初值，也就是須要粗配準，同時因爲算法自己缺陷，最終迭代結果可能會陷入局部最優，致使配準失敗，每每達不到咱們想要的效果。本文介紹的是另外一種比較好的配准算法，NDT配準。所謂NDT就是正態分佈變換，做用與ICP同樣用來估計兩個點雲之間的剛體變換。用標準最優化技術來肯定兩個點雲間的最優的匹配，由於其在配準過程當中不利用對應點的特徵計算和匹配，因此時間比其餘方法快。這個配准算法耗時穩定，跟初值相關不大，初值偏差大時，也能很好的糾正過來。

對比ICP配準方法須要提出不合適的點對，好比點對之間距離過大，包含了邊界點對，每次迭代都要搜索最近點，計算代價較大。正態分佈變換（NDT）算法是一種頗有用途的點雲配準方法，是一個一次性初始化工做，不須要消耗大量的代價計算最近鄰搜索匹配點，而且機率密度函數在兩幅圖像採集之間的額時間能夠離線計算出來，但仍在存在的問題不少，包括收斂域差、NDT代價函數的不連續性以及稀疏室外環境下不可靠的姿態估計等。

具體關於兩種方法的對比可查看文獻：http://www.diva-portal.org/smash/get/diva2:276162/FULLTEXT02.pdf

那麼針對無損檢測算法在二維和三維場景下的不足，也會有研究者們提出了相應的解決方法。爲了改進二維掃描配準的無損檢測收斂域，提出了一種多尺度K均值無損檢測（MSKM-NDT）算法，利用K均值聚類對二維點雲進行分割，並對多尺度聚類進行掃描配準優化。與標準無損檢測算法相比，k-均值聚類方法生成的高斯分佈更少，從而能夠評估全部高斯聚類的成本函數，從而保證算法的收斂性。因爲K均值聚類不能很好地擴展到三維，提出了分段貪婪聚類無損檢測（SGC-NDT）方法，做爲一種改進和保證三維點雲收斂的替代方法，該點雲包含與環境地面相對應的點。SGC-NDT算法使用高斯過程迴歸模型分割接地點，並使用貪婪方法對非接地點進行聚類。區域增加聚類算法提取環境中的天然特徵，生成高斯聚類，在無損檢測框架內用於掃描配準。涉及到的相關文獻以下：

Das A, Waslander SL. Scan Registration using Segmented Region Growing NDT. International Journal of Robotics Research. 2014.

Das A, Servos J, Waslander SL. 3D Scan Registration Using the Normal Distributions Transform with Ground Segmentation and Point Cloud Clustering. In 2013 IEEE International Conference on Robotics and Automation. Karlsruhe, Germany; 2013.

Das A, Waslander SL. Scan Registration with Multi-Scale K-Means Normal Distributions Transform. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Villamoura, Portugal; 2012.

 

 那麼接下里就具體介紹一下NDT算法的步驟

（1）該算法的第一步是將掃描佔用的空間細分爲單元格網格（2D圖像中的正方形或3D中的立方體），基於單元內的點分佈計算每一個單元的PDF。 每一個單元格中的PDF能夠解釋爲單元格內表面點x^的生成過程。將點雲投票到各個格子中，計算每一個格子的PDF，PDF能夠當作表面的近似表達，協方差矩陣的特徵向量和特徵值能夠表達表面信息（朝向、平整度） 格子內少於3個點，常常會協方差矩陣不存在逆矩陣，因此只計算點數大於5的cell，涉及到下采樣方法。

正態分佈給出了點雲的分段平滑表示，具備連續導數。 每一個PDF均可以看做是局部表面的近似值，描述了表面的位置以及它的方向和平滑度。 2D激光掃描及其相應的正態分佈如圖6.1所示。 圖6.2顯示了礦井隧道掃描的3D正態分佈。

 

 

 因爲目前的工做主要集中在正態分佈上，讓咱們更仔細地研究單變量和多變量正態分佈的特徵。 在一維狀況下，正態分佈的隨機變量x具備必定的指望值μ，而且關於該值的不肯定性用方差σ表示。

 

在多維的狀況下，平均值和方差由平均向量 μ和協方差來描述矩陣Σ。 協方差矩陣的對角元素表示方差每一個變量，非對角線元素表示的是協方差變量。 圖6.3說明了一維，二維和三維的正態分佈

（2）將第二幅掃描點雲的每一個點按轉移矩陣T的變換。

(3)第二幅掃描點落於參考幀點雲的哪一個 格子，計算響應的機率分佈函數

（4）當使用NDT進行掃描點配準時，目標是找到當前掃描點的位姿，以最大化當前掃描的點位於參考掃描表面上的可能性，該位姿是要優化的參數; 也就是說，當前掃描的點雲估計的旋轉和平移向量。求全部點的最優值，目標函數爲

 

2D-NDT

對於2D配準，有三個要優化的轉換參數。 讓p = [tx，ty，φ]T，其中 tx 和 ty 是平移參數，φ是旋轉角度。 使用逆時針旋轉，2D變換功能是

3D-NDT

 NDT的2D和3D配準的主要區別在於空間變換函數T（ p， x）及其偏導數。3D NDT使用歐拉角，有六個轉換要優化的參數：三個用於平移，三個用於旋轉。

可使用六維參數矢量對姿式進行編碼 p6 = [tx，ty，tz，φx，φy，φz]T

使用歐拉序列z-y-x，3D變換函數是

 

更具體的解釋可查看文獻

 在PCL中給出的NDT例子以下

http://pointclouds.org/documentation/tutorials/normal_distributions_transform.php#normal-distributions-transform