LiTAMIN2：一種超輕型LiDAR-SLAM（ICRA2021）

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

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

文章來(lái)源：ICRA2021論文（點(diǎn)云配準(zhǔn)）

視頻、論文地址：在公眾號(hào)「計(jì)算機(jī)視覺(jué)工坊」，后臺(tái)回復(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度量，顯著減少了點(diǎn)云配準(zhǔn)的點(diǎn)數(shù)，同時(shí)保持了配置精度。當(dāng)點(diǎn)數(shù)減少時(shí)，采用ICP法的點(diǎn)云配準(zhǔn)就不那么準(zhǔn)確了，因?yàn)镮CP基于的是點(diǎn)之間的距離最小化。為了避免這一問(wèn)題，將對(duì)稱(chēng)的KL-散度引入到反映兩個(gè)概率分布之間差異的ICP代價(jià)函數(shù)中。代價(jià)函數(shù)不僅包括點(diǎn)之間的距離，還包括分布形狀之間的差異。在KITTI數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，該方法具有很高的計(jì)算效率，性能優(yōu)于其他方法，并且與現(xiàn)有最先進(jìn)的SLAM方法具有相似的精度。

1 引言

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

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

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

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

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

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

2 相關(guān)工作

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

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

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

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

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

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

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

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

A.減少點(diǎn)數(shù)

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

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

B．應(yīng)用于對(duì)稱(chēng)KL散度的ICP代價(jià)函數(shù)

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)分析

A．對(duì)比

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

B.評(píng)價(jià)基準(zhǔn)和標(biāo)準(zhǔn)

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

根據(jù)以下三個(gè)標(biāo)準(zhǔn)評(píng)估每種方法的性能：

1） KITTI stats。KITTI Vision Benchmark [1]統(tǒng)計(jì)數(shù)據(jù)，即KITTI統(tǒng)計(jì)數(shù)據(jù)，用于準(zhǔn)確性評(píng)估。這些標(biāo)準(zhǔn)使得能夠使用相對(duì)于地面真實(shí)情況的相對(duì)關(guān)系來(lái)評(píng)估估計(jì)軌跡的質(zhì)量。在這項(xiàng)研究中，以這種方式針對(duì)不同的長(zhǎng)度（特別是每100 m至800 m）計(jì)算了平移和旋轉(zhuǎn)誤差，并計(jì)算了誤差的平均值。基準(zhǔn)提供的代碼用于計(jì)算KITTI統(tǒng)計(jì)信息。

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

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

C. Ablation study

所提出的方法將在每個(gè)體素中投****的子點(diǎn)云近似為正態(tài)分布。由于體素大小會(huì)顯著影響性能，因此針對(duì)不同體素大小對(duì)提議的方法進(jìn)行了全面評(píng)估，如表2所示。此外，表2顯示了使用體素化處理從原始掃描點(diǎn)開(kāi)始的平均減少百分比。從KITTI統(tǒng)計(jì)數(shù)據(jù)來(lái)看，體素越精細(xì)，準(zhǔn)確性就越不總是這種情況。請(qǐng)注意，在以下實(shí)驗(yàn)中，體素大小固定為3米，因?yàn)槭褂么酥悼色@得最佳性能。

D.對(duì)比分析

表3顯示了KITTI統(tǒng)計(jì)數(shù)據(jù)的比較。對(duì)于SuMa，比較了從作者的項(xiàng)目頁(yè)面獲得的軌跡數(shù)據(jù)集，幀到幀，幀到模型以及帶循環(huán)閉合的幀到模型。對(duì)于LeGO-LOAM和hdl-graph-slam，實(shí)現(xiàn)了閉環(huán)，但是由于在我們的實(shí)驗(yàn)中未進(jìn)行閉環(huán)檢測(cè)，因此結(jié)果未在表3中列出。對(duì)于LOAM，考慮了在他們的論文中使用開(kāi)源的測(cè)量結(jié)果以及原始論文的結(jié)果。原始序列中列出了各個(gè)序列的統(tǒng)計(jì)信息，但最終的KITTI統(tǒng)計(jì)數(shù)據(jù)未在表3中列出，因?yàn)樗鼈兾丛谠颊撐闹辛谐觥O-Net未在表3中列出，因?yàn)樵赱22]中提供了各個(gè)序列的統(tǒng)計(jì)信息，但未提供所有最終誤差值的平均值。

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

總結(jié)

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