ICP問題
匹配未考慮形狀，會陷入局部最小值，受形狀影響大，魯棒性不夠好。換言之精度會失效。
馬氏距離引入
將點雲和位姿矩陣看做概率量（隨機變量），引入馬氏距離評定（ICP為歐式距離評定），由於協方差矩陣可以度量多個維度上的信息（公式0-1、公式0-2、公式0-3），故可以刻畫多維信息，從理論上我們就可以消除ICP無形狀考慮的問題（魯棒性不夠好、精度失效）
（公式0-1） 一維只有方差
.（公式0-2） 二維協方差
（公式0-3） 三維協方差矩陣
NDT改善
通過某區域內（划分網格內）概率分布可以得到點雲的形狀分布，
（公式1）
上述公式可以看做歸一化的馬氏距離，質心和協方差可以表示出各點和點雲中心位置與各點雲方向（得益於協方差），故可以表示點雲位置形狀等豐富信息。（另外注意到上述對馬氏距離取了負，故越遠越接近0，越近越接近1）
上圖為通過概率刻畫點雲，左邊為隧道原始點雲，右邊為每個方塊內點的概率（公式1）表示越接近1的顏色越淺，以上得益於馬氏距離的信息豐富性，可以看出擬合后形狀是比較相近的（這是一個很大的進步）。
上圖為一維、二維、三維的概率（馬氏距離）表示。
一維中橫坐標為一維x位置，縱坐標為概率值（馬氏距離），值越大（馬氏距離越小）越接近中心，概率代數值越接近1，拍到一維直線上，用顏色表示大小構成二維信息，顏色越淺越接近一；
二維中有兩個方向的度量，同樣橫縱坐標為二維xy位置顏色表示概率值，也就是說概率是復合結果，二維馬氏距離展開會有x與y的相關項，cov（x，y）也刻畫了另一維影響
特別的：當其中一維很接近這一維的質心時時會退化成一維形式（如：考慮y=v則+的右項=0，且cov（x,y）=0），也就是說離哪一維更近哪一維就起主要作用。
三維中有三個方向的度量，為了表示三維加入坐標軸，坐標軸壓縮量表示了點雲分布的大致形狀。特別的：其中一維接近這一維的質心時會退化為二維，兩維接近時會退化成一維。
注：加入退化情況說明是為了方便理解協方差所起作用
從以上可以看出表面方向和光滑度可以從協方差矩陣的特征向量值和特征值來評估。
我們求以下最優問題即可
上圖的意思也就是，通過給定位姿p將點雲x下標第k個通過T函數之后，求相應的概率值。
我是分割線---------------------------------------------------------------------------------------------------我是分割線
以下是構建方程技巧trick部分，可以跳過
首先是求優化問題argmin的通用求法，取負
然后對於較遠點雲的影響（近0值），乘以一系數c1以減小其影響也就是增大exp（），我們預估較遠點比例
為55%然后c1=10*（1-55%） c2=比例/選取點雲體積
注：較遠點的影響是分割網格大小的關鍵，對於較大的網格，較遠點影響會加劇
上圖中
是取系數之后的概率值，可以看到在一維情況下消除了無窮大值（較遠點概率值取負log值）的影響
然后進行高斯擬合，也就是
將
變為
將d1、d2、d3摘出來
其中有三個未知量，我們取三個特殊值值即可解這個三元一次方程，分別是x=0、x=σ、x=∞
以上可以大大減小我們的計算量，快速得到d1、d2、d3
得益於高斯近似，現在我們的優化方程變為
對於等式右邊的記號我們從新記一個，叫做得分記為s，接下來最優化s求解位姿p就行了
我是分割線---------------------------------------------------------------------------------------------------我是分割線
談完了方程構建，接下來到了求解部分，依舊是trick部分，可以跳過
首先我們求
但是三個點構成一個平面，這樣就確實了一維的特征值，變成奇異矩陣不滿秩，所以我們只對大於5個點的網格進行計算。
另外對於特殊情況、降維問題，若最大的特征值比最小的大100倍，我們認為降維了，將最小特征向量的尺度變為最大特征值/100，這種倍數關系可以優化計算。
求解高斯牛頓法二階，專門寫了一篇高斯牛頓相關的文章，如果有不懂的可以點擊這里：this。
其中梯度（雅克比矩陣/一階導數矩陣）矩陣為
海塞矩陣（二階導數矩陣）
為
梯度詳細（雅克比矩陣/一階導數矩陣）：
黑塞詳細（二階導數矩陣）：
我是分割線---------------------------------------------------------------------------------------------------我是分割線
以上終於講完了原理部分：
最終我們整理一遍具體算法流程：
首先對范圍內的三維點雲進行三維網格（1m³為例）划分，也就是進行下采樣，下采樣之后會記錄網格內的協方差（形狀）
搜索目標點雲的最近鄰網格（下采樣后的）
根據搜索到的點的網格參數計算與馬氏距離相關的高斯概率總和，迭代優化馬氏距離求解旋轉平移矩陣，直到馬氏距離最小（形狀最相近）的即是所找到的位姿矩陣。
我是分割線---------------------------------------------------------------------------------------------------我是分割線
精度方面ICP匹配對平移/各點雲相對偏移魯棒性更好，NDT對旋轉/形狀魯棒性更好，精度也更高。下一篇談論時間（ICP耗時很短、NDT耗時比較長）