(文獻閱讀) LiTAMIN2: Ultra Light LiDAR-based SLAM using Geometric Approximation applied with KL-Divergence

論文作者：Masashi Yokozuka, Kenji Koide, Shuji Oishi, Atsuhiko Banno

文章來源：ICRA2021論文（點云配準）

視頻、論文地址：在公眾號「計算機視覺工坊」，后臺回復(fù)「LiTAMIN2」，即可直接下載。

其中的版本1，LiTAMIN: LiDAR-based Tracking And Mapping by Stabilized ICP for Geometry Approximation with Normal Distributions.

可參考博客：https://blog.csdn.net/Yong_Qi2015/article/details/114528734；

論文鏈接：https://ieeexplore.ieee.org/document/9341341

翻譯：chaochaoSEU

摘要：本文提出了一種可用于500-1000hz處理的3D LiDAR SLAM方法。該方法利用一種新的ICP度量，顯著減少了點云配準的點數(shù)，同時保持了配置精度。當點數(shù)減少時，采用ICP法的點云配準就不那么準確了，因為ICP基于的是點之間的距離最小化。為了避免這一問題，將對稱的KL-散度引入到反映兩個概率分布之間差異的ICP代價函數(shù)中。代價函數(shù)不僅包括點之間的距離，還包括分布形狀之間的差異。在KITTI數(shù)據(jù)集上的實驗結(jié)果表明，該方法具有很高的計算效率，性能優(yōu)于其他方法，并且與現(xiàn)有最先進的SLAM方法具有相似的精度。

1 引言

同時定位與建圖（SLAM）是移動技術(shù)和服務(wù)的一個基本元素，如自主移動機器人。特別是，LiDAR和深度傳感器已經(jīng)被商業(yè)化，且由于其穩(wěn)定和準確的性能而正在被應(yīng)用。在不久的將來，不僅是自動駕駛的汽車，而且所有類型的移動設(shè)備都將配備激光雷達或深度傳感器。我們預(yù)計，在這樣一個世界中，通過SLAM獲取的點云數(shù)據(jù)將被聚合到云端并共享以提供各種服務(wù)。

有必要從全球的設(shè)備中實時聚合的大量點云數(shù)據(jù)中有效地生成和更新世界地圖。由于在此過程中使用的服務(wù)器的數(shù)量比設(shè)備的數(shù)量要小得多，因此必須使用超出實時性能的SLAM方法。目前LiDAR-SLAM的性能只稍微好于實時性能。

除了服務(wù)器之外，操作邊緣設(shè)備也需要加速，而邊緣設(shè)備在計算資源方面受到了嚴重的限制。目前LiDAR-SLAM是基于使用CPU和GPU的PC保證實時性，提高SLAM方法的計算效率以確保邊緣設(shè)備的實時性。

雖然許多研究已經(jīng)在SLAM基準 [1]和追求精度的方法[2]-[14]上進行，但很少有研究已經(jīng)顯著提高了目前的計算效率。在未來，要集中、高效地處理大量的機器人和設(shè)備，SLAM方法將更加追求高速。

本文的目的是建立一種與最先進方法一樣準確的方法，同時實時性顯著超過當前方法。本文討論了一種3D LiDAR-SLAM，其顯著提高了LiDAR-SLAM的計算效率，可在500-1000 Hz下運行，且提供了與最先進方法相同的精度。該方法在保持精度的同時，使用一種新的ICP度量，顯著減少了用于點云配準的點數(shù)。采用ICP的點云配準方法在點云數(shù)較少時，精度低。為了避免這個問題，引入了對稱的ICP代價函數(shù)。實驗結(jié)果（圖1)表明，該方法具有很高的計算效率，性能優(yōu)于其他方法，并與最先進SLAM方法有相似的精度。

圖1 使用LiTAMIN2的KITTI數(shù)據(jù)序列00的示例建圖結(jié)果。右下角圖的顏色表示由主成分分析分解的正態(tài)分布給出的正態(tài)方向，該方向是最小特征值的特征向量。

2 相關(guān)工作

LiDAR-SLAM方法可以分為兩類：基于 ICP的方法[2]-[8]和基于特征的方法[9]-[14]。

對于基于ICP的方法而言，體素化是一種簡單而有效的加速方法。通過將點云劃分為小群，并以正態(tài)分布逼近每個子點云，可以在一定程度上在保持形狀的同時顯著減少點的數(shù)量。正態(tài)分布變換(NDT)[15]-[17]和GICP[18]是執(zhí)行體素逼近的最常用的icp方法，但它們之間存在一些區(qū)別。NDT只逼近具有正態(tài)分布的目標點，并確定體素對應(yīng)，而GICP對目標點云和源點云進行正態(tài)分布逼近，并使用KD樹的精確最近鄰搜索尋找相關(guān)性。NDT的計算效率往往更高，而GICP則往往更準確。

基于特征的方法是從LiDAR數(shù)據(jù)中提取點、線、面等幾何特征，并有效地確定對應(yīng)關(guān)系。LOAM[9]是第一個使用特征方法進行快速和準確的LiDAR里程計。它使用特征匹配顯著減少了定位階段所需的點數(shù)。LeGo-LAOM [11]只依靠良好的性能來執(zhí)行特性選擇，是目前可用的最快方法之一[2]。

為了實現(xiàn)更快的配準，一些方法利用GPU的計算能力，包括SuMa[4]、Elastic-Fusion [6]、Elastic-LiDAR Fusion [5]和Droeschel等的方法[8]。它們將LiDAR數(shù)據(jù)的形狀近似為Surfel (面元)[19]。Surfel是一種基于點的渲染方法[20]，它旨在用點云而不是多邊形網(wǎng)格來渲染三維形狀，并且適用于GPU處理。因此，它允許通過使用硬件支持的投影數(shù)據(jù)關(guān)聯(lián)來實現(xiàn)快速的點對平面ICP[21]的性能。

基于深度神經(jīng)網(wǎng)絡(luò)的LiDAR里程計[22]-[24]的方法也已經(jīng)流行起來。LO-Net[22]是一種端到端的LiDAR里程計。雖然用于訓(xùn)練和測試的數(shù)據(jù)變化較少，但它顯示的精度與傳統(tǒng)方法相似。LO-Net主要是卷積張量運算，這使得點對點處理的并行化更加容易，對GPU的未來演化也可擴展。然而，端到端里程計估計是否可在未學(xué)習(xí)的環(huán)境和運動下工作尚未得到足夠的研究，因此需要進一步的研究。

當前的LiDAR SLAM方法大約需要N點數(shù)的O（N）或O（Nlog（N）），并且從理論上講，應(yīng)該引入不同的方法來改進算法。本研究中的主張簡單明了：基于ICP的方法所需的點數(shù)應(yīng)該少。通常，基于ICP的方法的準確性會隨著點數(shù)的減少而降低；因此，應(yīng)該找到一種方法來解決速度和精度之間的權(quán)衡問題。

3 數(shù)學(xué)模型

在本節(jié)中，我們描述了該方法與LiTAMIN[2]之間的差異；這些差異是用于減少點數(shù)的方法和使用ICP代價函數(shù)。

A.減少點數(shù)

如圖2所示，LiTAMIN將一組輸入點投****到體素網(wǎng)格中，使用投****點的方法對齊它們，并將點云集成到體素圖中。該方法以類似的方式執(zhí)行SLAM，但不同之處在于，它對輸入點組的投****結(jié)果使用了協(xié)方差，而不僅僅是平均值。而LiTAMIN是一種point-to-normal分布建圖，該方法將其擴展到distribution-to-normal分布建圖。這是為了通過考慮分布來提高準確性。該方法增加了每個體素的大小，減少了點數(shù)，這大大降低了計算成本。此外，它通過考慮分布的形狀而不是點來避免了精度的損失。

圖2 我們的方法概述。右圖的顏色表示通過使用主成分分析分解的正態(tài)分布給出的法線方向；顏色類似于圖1中的顏色。

B．應(yīng)用于對稱KL散度的ICP代價函數(shù)

表1顯示了現(xiàn)有方法和所提方法的ICP代價函數(shù)。該方法與其他方法的區(qū)別在于，代價不僅考慮了點之間的距離，還考慮了分布的形狀。雖然其他的方法，如NDT[15]，GICP[18]和LiTAMIN [2]，已經(jīng)考慮了協(xié)方差，但在實踐中，它們只通過加權(quán)協(xié)方差的倒數(shù)來評估距離。該方法同時計算第一項的加權(quán)距離和第二項的分布形狀差異。例如，如果點之間的距離很小，但分布的形狀不匹配，則代價函數(shù)被設(shè)計為很大。這個代價來自于兩個高斯分布p和q[25]的KL-散度DKL(p||q)[26]：

KL-散度是對分布之間差異的一種度量，它不僅代表了平均值之間的差異，而且還代表了分布形狀之間的差異。KL-散度的目的是進行一個考慮到分布形狀的魯棒配準。

然而，KL-散度DKL(p||q) ≠DKL(q||p)并不對稱：它通常被認為不是一個距離。由于ICP是一種距離最小化算法，需要更合適的度量，因此使用DSymKL(p||q)，本研究引入了以下對稱性：

通過在DSymKL(p||q)中應(yīng)用歸一化，并引入剛體變換R和t，得到了代價函數(shù)，為了進一步解決異常值，ICP誤差EICP和分布形狀誤差ECOV設(shè)置如下：

另外，其權(quán)重如下：

在表1中，考慮到LiTAMIN的兩個分布的協(xié)方差，所提出的方法的第一項可以看作是ICP代價。與LiTAMIN主要區(qū)別是第二個代表分布形狀差異的術(shù)語。在本研究中，引入這個項，使精度不會降低，即使點的數(shù)量因為大的體素大小而大大減少。它可以只計算第一項，即ICP代價。因此，研究了僅是第一項的情況與實驗中第一項和第二項組合的情況之間的差異。

C.實現(xiàn)和參數(shù)

本研究利用牛頓方法優(yōu)化了該方法的代價函數(shù)。由于代價函數(shù)的第二項不是平方誤差，它需要先求Hessian，并且使用牛頓法，而不是高斯-牛頓方法。這項研究沒有使用LM法[27]的阻尼因數(shù)，因為沒有它的計算是穩(wěn)定的。

σICP和σCov的可接受值分別根據(jù)經(jīng)驗設(shè)置為0.5和3。sigmaICP對應(yīng)于到Cqp的Mahalanobis距離，這意味著低于0.5的對應(yīng)點是可信任的。σCov對應(yīng)于ECOV；如果Cq和RCpRT相同，則ECOV = Tr（I）+ Tr（I）-6應(yīng)該為0。在DSymKL(p||q)中，-2d是使DSymKL的最小值為0的項。將σCov= 3設(shè)置為允許在沒有該術(shù)語的情況下允許的大約一半的誤差。如在LiTAMIN中一樣，F(xiàn)robenius歸一化的參數(shù)lambda設(shè)置為10-6。

LiTAMIN使用權(quán)重w的占用概率[28]，而所提出的方法使用wICP和wCov代替。對于閉環(huán)檢測，使用了建議的ICP代價。其他元素使用與LiTAMIN中相同的參數(shù)和實現(xiàn)。除LiTAMIN外，還使用KD樹搜索了ICP的相應(yīng)點。關(guān)于圖的表示，除了LiTAMIN以外，還使用了體素圖。體素貼圖用于減少構(gòu)成貼圖的正態(tài)分布的數(shù)量。

4 實驗分析

A．對比

幾種最先進的方法被選擇為使用不同加速算法的競爭對手，特別是LiTAMIN、 SuMa、 LeGO-LOAM、 LOAM、 hdl-graph-slam[3], LO-Net 和 DeepLO。在SuMa項目頁面上提供了一個詳細的評估，因此在研究中提到了這個問題，而計算時間是由運行開源的研究人員獲得的。在接下來的實驗中，我們還利用LeGO-LOAM、LOAM、hdl-graph-slam的開源來獲得軌跡和測量計算速度。關(guān)于LO-Net，使用了原論文的結(jié)果。

B.評價基準和標準

實驗中使用了KITTI視覺基準測試，它包含了Velodyne HDL-64E S2在多個環(huán)境中獲取的點云。因此，它允許評估由任何SLAM方法獲得的軌跡。所提供的點云已經(jīng)被分解，因此它們被直接輸入到所提出的方法和競爭對手中。

根據(jù)以下三個標準評估每種方法的性能：

1） KITTI stats。KITTI Vision Benchmark [1]統(tǒng)計數(shù)據(jù)，即KITTI統(tǒng)計數(shù)據(jù)，用于準確性評估。這些標準使得能夠使用相對于地面真實情況的相對關(guān)系來評估估計軌跡的質(zhì)量。在這項研究中，以這種方式針對不同的長度（特別是每100 m至800 m）計算了平移和旋轉(zhuǎn)誤差，并計算了誤差的平均值?；鶞侍峁┑拇a用于計算KITTI統(tǒng)計信息。

2）Absolute Trajectory Error（ATE）：為了評估每種方法的閉環(huán)性能，還計算了ATE [29]。ATE是相對于地面真實情況的絕對位置和姿態(tài)誤差的指示器。KITTI統(tǒng)計信息是子軌跡誤差的平均值，可能會低估閉環(huán)的影響；但是，ATE允許基于絕對誤差評估比較由閉環(huán)修改的軌跡的整個形狀。

3）Total time and frame rate：作為計算效率的指標，計算了處理KITTI Vision Benchmark的所有序列（包括閉環(huán)）所花費的總時間。還介紹了里程計的幀頻，以評估位置估計的速度，這對于某些實時應(yīng)用可能很重要。

C. Ablation study

所提出的方法將在每個體素中投****的子點云近似為正態(tài)分布。由于體素大小會顯著影響性能，因此針對不同體素大小對提議的方法進行了全面評估，如表2所示。此外，表2顯示了使用體素化處理從原始掃描點開始的平均減少百分比。從KITTI統(tǒng)計數(shù)據(jù)來看，體素越精細，準確性就越不總是這種情況。請注意，在以下實驗中，體素大小固定為3米，因為使用此值可獲得最佳性能。

D.對比分析

表3顯示了KITTI統(tǒng)計數(shù)據(jù)的比較。對于SuMa，比較了從作者的項目頁面獲得的軌跡數(shù)據(jù)集，幀到幀，幀到模型以及帶循環(huán)閉合的幀到模型。對于LeGO-LOAM和hdl-graph-slam，實現(xiàn)了閉環(huán)，但是由于在我們的實驗中未進行閉環(huán)檢測，因此結(jié)果未在表3中列出。對于LOAM，考慮了在他們的論文中使用開源的測量結(jié)果以及原始論文的結(jié)果。原始序列中列出了各個序列的統(tǒng)計信息，但最終的KITTI統(tǒng)計數(shù)據(jù)未在表3中列出，因為它們未在原始論文中列出。LO-Net未在表3中列出，因為在[22]中提供了各個序列的統(tǒng)計信息，但未提供所有最終誤差值的平均值。

表4表示ATE的比較。除表2外，還對SuMa的結(jié)果進行了評估，分析了從作者的項目頁面獲取的軌跡。對于LOAM，將顯示開源軟件的結(jié)果。LO-Net被排除在表4中，因為ATE的結(jié)果未包含在原始論文中。表5顯示了使用KITTI的每個序列和測距法過程的平均幀速率創(chuàng)建地圖所花費的處理時間。所提出的方法LiTAMIN和SuMa顯示了包括循環(huán)閉合在內(nèi)的處理時間，因為這些方法中的循環(huán)閉合對所有序列都是成功的 SuMa結(jié)果是在本研究使用的計算機上使用開放源代碼獲得的，因為SuMa原始論文中的計算機規(guī)格不同。圖3顯示了每種方法的軌跡比較。左起的第二和第四幅圖顯示了所提出的方法和SuMa的閉環(huán)結(jié)果，但未顯示其他無法檢測到環(huán)路的方法。

總結(jié)

在這項研究中，提出了一種使用對稱KL散度的ICP方法來顯著提高LiDAR-SLAM的速度，并將其與其他最新的SLAM方法進行了比較。所提出的方法在里程計幀速率上實現(xiàn)了500 fps至1000 fps的計算速度，并且具有與其他方法相同的準確度，這證實了所提出的方法是從傳統(tǒng)方法向前邁出的一大步。這是因為通過將點云從LiDAR投****到每個體素網(wǎng)格并將投****的子點云近似為一個正態(tài)分布，從而大大減少了用于配準的點數(shù)。盡管所提出的方法大大減少了點數(shù)，但是所提出的對稱KL散度的ICP成本允許在不降低準確性的情況下處理數(shù)據(jù)。這些結(jié)果基于KITTI Vision Benchmark數(shù)據(jù)集，我們認為我們需要研究當使用環(huán)境發(fā)生變化時如何確定適當?shù)捏w素大小。