点云配准ICPNDT
NDT
簡介
很多匹配算法需要在兩幀數據之間進行特征匹配，例如ICP中會進行點到點、點到線、點到面的特征匹配，特征匹配的效果最終決定了點云配準的效果，而NDT不需要進行特征匹配。NDT是將第一幀點云轉換至柵格地圖，每個柵格計算其中點的正態分布，因此將第一幀點云轉換為一個個柵格表示的分段連續可導的概率密度函數，使用正態分布概率密度函數描述點云的局部性質。然后將第二幀點云投影至柵格地圖中，計算出第二幀點云在柵格地圖中的概率，通過牛頓法找到最佳匹配位姿，使得第二幀點云投影至柵格地圖后概率最大化，最終完成點云配準。
NDT的步驟可分為：建立第一幀點云的柵格地圖，牛頓法迭代使得第二幀點云投影至柵格地圖得到的概率最大化。
NDT優點：不需要進行點云之間的特征匹配，避免了特征匹配中出現的問題，例如點云噪聲、物體移動、點云重合度對特征匹配的影響。使用正態分布概率密度函數描述點云的局部性質，因此所有導數都可以通過解析法求解，因此計算結果準確并且比較快。
建立柵格地圖
NDT通過一系列柵格描述的本地點云正態分布描述整個點云的分布。首先將第一幀點云分割為等大的柵格，對于包含一定數量點的柵格，計算柵格內點坐標的均值以及方差，最終可以獲得本柵格內點云的正態分布：
因此每個柵格描述了柵格中任意位置的概率密度。
實現細節
由于柵格地圖將整個點云進行了離散化，在柵格與柵格之間會出現不連續的現象，可能會造成誤差。為了避免離散化的影響，NDT會同時建立四個柵格地圖，彼此相互錯位半個柵格的長度，這四個柵格地圖會同時參與計算，最終結果為四個柵格地圖計算結果的和。
計算柵格地圖內協方差矩陣時，為了避免協方差矩陣出現奇異性，如果協方差矩陣較大特征值遠大于較小的特征值，則將較大特征值進行減小，最終計算得到協方差矩陣。
掃描匹配
首先根據里程計或者勻速運動模型得到先驗位姿，根據先驗位姿將第二幀點云投影至柵格地圖中，對于第二幀點云中每個點，根據它所在的柵格計算概率密度，最終通過求和得到第二幀點云在柵格地圖中的概率密度，點云位姿作為參數塊，使用牛頓法找到這個概率密度的最大值，最終得到后驗位姿。
由于使用正態分布概率密度函數描述點云的局部性質，因此概率密度函數的導數和偏導數可以通過解析法進行求解，所以可以計算出牛頓法中所需要的海塞矩陣和雅克比矩陣。
牛頓法迭代公式推導
第二幀點云在柵格地圖中的概率密度為：
對概率密度取負號，通過牛頓法找到最小值。對于牛頓法每次迭代，需要求解增量方程：
因此需要求解海塞矩陣和雅克比矩陣，得到增量后更新位姿，不斷迭代，直到增量位姿小于一定閾值，說明收斂。
對于點云中一點，他在柵格內關于均值的坐標為：
因此這個點的概率密度為：
通過鏈式求導，概率密度關于位姿的導數等于概率密度關于點激光點局部坐標的導數乘以激光點局部坐標有關位姿的導數，因此可得到雅克比矩陣
其中，激光點局部坐標有關位姿的導數為：
海塞矩陣:
NDT算法特點：
ICP
簡介
ICP通過迭代的方式，通過優化相對位姿，不斷減小兩幀點云之間點到點的距離，直到迭代收斂，最終計算出兩幀點云的相對位姿。ICP分為特征匹配、位姿優化兩個步驟。
由于無法像視覺SLAM提供準確的特征匹配關系，因此對于基于激光點云的經典ICP來說，使用點到點的最近距離進行特征匹配。使用ICP算法配準源點云和目標點云，對于源點云中每個點，找到它在目標點云中最近的點，如果這兩個點的距離小于閾值，則將這兩個點作為匹配點，計算出所有匹配點之后，最終可以獲得源點云與目標點云的匹配關系。
獲得兩點云的匹配點之后，構建非線性最小二乘問題，參數塊是兩點云之間的相對位姿，殘差是所有匹配點之間的距離的和，可以通過優化或者解析的方法計算最優位姿變換（例如基于SVD的解析方法），并不斷進行特征匹配、位姿優化的迭代，直到滿足收斂條件，最終計算出兩點云的相對位姿。
位姿優化
公式推導思想
首先計算兩點云的去質心坐標點云，然后兩點云的旋轉由去質心坐標點云求解，兩點云的位移由經過旋轉后的質心坐標求解，因此將將旋轉和位移的求解分割為兩個步驟。
詳細推導
對于點云中每個匹配特征點，代價函數為：
由于兩幀點云完成匹配后相互重合，因此兩點云的質心坐標是相同的，因此首先計算兩點云的質心坐標，然后計算出質心坐標下的兩個點云
所以代價函數就成為質心坐標下的兩點云匹配點的和：
所以將最小二乘問題轉換為兩步，首先根據質心坐標下的代價函數求得相對姿態，然后相對位置就是經過相對姿態變換后兩點云坐標的差
計算
對H矩陣進行SVD分解，計算
如果行列式為1，說明X為相對姿態，然后求得相對位姿
SVD公式推導
思想
最后代價函數可轉換為旋轉矩陣與去質心坐標矩陣乘積的跡，由定理可知，正定矩陣的跡要大于任意正交矩陣乘以正定矩陣的跡，因此找到旋轉矩陣R，可轉換為正定矩陣的行駛，就求得了所需要的旋轉矩陣。
對H矩陣進行SVD分解，取正交矩陣為SVD分解后的矩陣，然后回帶，會發現轉換為了正定矩陣的形式。
詳細推導
質心坐標系下代價函數可寫為
可以看到，最小化代價函數等價于最大化
通過對H的SVD分解，可以得到
因此令R等于
即可求得非線性最小二乘問題的最小值
TODO：如何理解通過SVD求最大值？
ICP和NDT算法之間的優劣
ICP相比NDT，更容易受到初值誤差的影響，當初始誤差較大時，ICP更容易出現無法計算位姿的情況。
NDT相比ICP，旋轉誤差更大。因此ICP對旋轉的約束更強，NDT對初始誤差的健壯性更強。[1]
NDT的匹配速度要比ICP塊，NDT的時間復雜度為O(N)，其時間復雜度與點云的數量成正比，ICP的時間復雜度是O(NlogN)，即通過KD樹進行查找的時間復雜度。
由于ICP是進行點云中點到點的匹配，而NDT是將點云轉換為分段連續的正態分布函數，因此NDT對動態物體干擾的抵抗力更強。
由于NDT對初始誤差的敏感度較低，因此當速度較快時，運動方程無法提供準確的先驗位姿，此時NDT的效果要由于ICP。
當環境中缺少垂直特征時，NDT相比ICP會提供更好的約束，原因是NDT使用柵格正態分布描述點云的局部性質，因此稀少的垂直特征可以體系在某些柵格的正態分布中，而ICP使用點云之間點到點的匹配，垂直點特征的影響會被大面積水平點特征減弱。[2]
[1]Evaluation_of_3D_registration_reliability_and_speed_-_A_comparison_of_ICP_and_NDT
[2]3D Scan Registration Based Localization for Autonomous Vehicles – A Comparison of NDT and ICP under Realistic Conditions
ICP要求點云的完全重合，這一假設會受到現實各種因素的影響。
點云配準不利因素
- 初始值誤差
- 點云的不重合度
- 動態物體
傳統ICP及其變體的比較
傳統ICP的缺陷
由于經典ICP計算點云中點到點的距離，因此最小二乘期望的是兩個點云的完全重合，然而現實環境中由于遮擋關系，兩幀點云往往不是完全重合的，可能兩幀點云只存在一部分的點是完全重合。由于點云實際上是現實幾何環境的離散化表示，同時點云受到噪聲的影響，即便在同一平面，離散化得到的點云也不可能完全相同，因此使用點到點的距離無法準確描述點云之間的配準問題。
點到面、點到線的距離解決了離散化問題。由于點云中的平面只提供了平面法向的約束，因此計算點到面的距離，只考慮平面法向約束，而避免了平面切向方向的影響。
改進的ICP——GICP和NICP
GICP是以概率的視角解決ICP問題，通過求解最大似然，求解出兩點云的相對位姿，GICP充分地考慮了點附近的結構信息，因此可以在非線性優化過程中避免很多錯誤的匹配。
GICP的基本流程與ICP基本相同，首先通過KD樹尋找歐式距離最小的點作為兩點云之間的匹配。通過高斯分布的概率模型來描述每個激光點，計算出激光點的協方差矩陣，然后通過最大似然，構建出相應的非線性最小二乘問題，根據匹配點的各自的協方差，誤差函數的距離進行加權，也就是通過協方差矩陣構建出的馬氏距離，描述非線性最小二乘的誤差。
計算協方差矩陣
通過KD樹，找到該點附近的激光點，首先計算出激光點的均值，然后再計算協方差矩陣，由于GICP基于平面假設，認為點云是在連續平面上的離散采樣，因此會對計算出的協方差矩陣進行SVD分解，通過修改奇異值為[1,1,小量]，將該點的協方差矩陣修改為滿足平面分布的協方差矩陣.
最大化似然，求解非線性最小二乘
優點
由于GICP充分考慮了點的局部平面性質，對于兩對匹配激光點，通過其各自協方差矩陣的加權，只會在法線方向提供約束，也就是兩點之間法線方向的距離會有較大的權重，平面方向權重很小。并且對于錯誤匹配點對，根據各自的協方差在非線性優化中加權，得到的權重較小，從而避免了錯誤匹配點對在非線性優化的影響，例如兩個匹配點對各自的平面是相互垂直的，屬于錯誤匹配，因此通過非線性優化加權，得到的權重較小。
總結
以上是生活随笔為你收集整理的点云配准ICPNDT的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： rdkitpython | 通过反应获得
- 下一篇： 如何测试静音检测