3D激光匹配算法

• 1 基本方法
• • 1.1 點云表示
  • 1.2 ICP方法
  • • 1.2.1 問題建模
    • 1.2.2 算法流程
    • 1.2.3 旋轉計算(SVD)
  • 1.3 NDT方法
  • • 1.3.1 預處理
    • 1.3.2 問題建模
    • 1.3.3 優化求解
  • 參考資料

1 基本方法

1.1 點云表示

點云數據中包含了每個點的經緯度坐標、強度、多次回波、顏色等信息。3D激光點云數據里所包含的信息：
（1）x.y.z坐標信息
根據采集的x.y.z坐標，可直接獲取被測物體的三維結構信息，三維結構信息是其他地理信息的載體。
（2）回波次數
回波數是某個給定脈沖的回波總數。從激光雷達系統發射的激光脈沖會從地表面和地表上的物體反射：汽車、行人、以及橋梁等等。
發射出的一個激光脈沖可能會以一個或多個回波的形式返回到激光雷達傳感器。任何發射出的激光脈沖在向地面傳播時，如果遇到多個反射表面則會被分割成與反射表面一樣多的回波。
(3) 強度信息
強度是反映生成某點的激光雷達脈沖回波強度的一種測量指標(針對每個點而采集)。不同物體對于激光雷達的反射程度不同，所以通過強度信號可以對物體進行區分。
(4) 類別
每個經過處理的激光雷達點可擁有定義反射激光雷達脈沖對象類型的分類。可將激光雷達點分成多個類別，包括汽車、路面等。
(5) RGB
可以將RGB(紅、綠、藍)波段作為激光雷達數據的屬性。此屬性通常來自激光雷達測量時采集的影像。
(6) 掃描方向
掃描方向是激光脈沖向外發射時激光掃描鏡的行進方向。

用數學的方式描述：
(1) 空間中的一個點：

（2）所有的點構成點云：

點云質心:
點云的重心是一個點坐標，計算出云中所有點的平均值, 可以說它是“質量中心”，認為是物體質量集中于此點的假想點。
物體質心公式：

在點云中，各點質量為1:

1.2 ICP方法

1.2.1 問題建模

問題模型：求解兩幀點云(P和Q)之間的變換關系，找到最佳的旋轉R和平移t, 使得經過變換后兩幀點云之間的距離誤差最小。
假設3D傳感器在不同位置下采集兩幀點云：

轉化為最小二乘優化問題：

在誤差函數中加入質心：

1.2.2 算法流程

計算流程如下：

1.2.3 旋轉計算(SVD)

按照上面過程，只需要重點解決R的計算，就可以解決平移，接下對R的優化函數進行展開簡化：

關于SVD分解的原理可以看《矩陣論》里面的矩陣分解章節，不進行具體介紹。

1.3 NDT方法

NDT是正態分布變換算法，基本思想是先根據參考數據（reference scan）來構建多維變量的正態分布，如果變換參數能使得兩幅激光數據匹配的很好，那么變換點在參考系中的概率密度將會很大。因此，可以考慮用優化的方法求出使得概率密度之和最大的變換參數，此時兩幅激光點云數據將匹配的最好。
有原始兩幀點云數據，其中參考數據點云為X：

1.3.1 預處理

劃分網格，計算統計特性

1.3.2 問題建模

構造最小誤差問題模型

從形式來看，NDT形式是多維正態分布的極大似然函數，NDT匹配計算的本質就是一個多維正態分布的極大似然估計。

1.3.3 優化求解

優化方法在實現上采用了牛頓法，但是為了增加魯棒性，只采用了牛頓法計算出來的迭代方向作為步長搜索的初始方向。
優化問題屬于非線性優化的基本問題，可以參照相關大學或研究生優化課程，不詳細介紹。非線性優化方法小結

改進NDT
在點云預處理階段，采用不同的采用策略，比如隨機采用，均勻采樣，法空間采樣等，提高效率。
在柵格策階段，采用從固定尺寸柵格到基于八叉樹的變化尺寸策略。

參考資料

[1] https://www.cnblogs.com/lvchaoshun/p/7823306.html
[2] https://zhuanlan.zhihu.com/p/56598915
[3] https://www.cnblogs.com/YongQiVisionIMAX/p/13871721.html

總結

以上是生活随笔為你收集整理的SLAM算法 - 3D激光匹配算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。