3D激光匹配算法

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

1 基本方法

1.1 點云表示

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

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

（2）所有的點構(gòu)成點云：

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

在點云中，各點質(zhì)量為1:

1.2 ICP方法

1.2.1 問題建模

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

轉(zhuǎn)化為最小二乘優(yōu)化問題：

在誤差函數(shù)中加入質(zhì)心：

1.2.2 算法流程

計算流程如下：

1.2.3 旋轉(zhuǎn)計算(SVD)

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

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

1.3 NDT方法

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

1.3.1 預(yù)處理

劃分網(wǎng)格，計算統(tǒng)計特性

1.3.2 問題建模

構(gòu)造最小誤差問題模型

從形式來看，NDT形式是多維正態(tài)分布的極大似然函數(shù)，NDT匹配計算的本質(zhì)就是一個多維正態(tài)分布的極大似然估計。

1.3.3 優(yōu)化求解

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

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

參考資料

[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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。