LT-mapper系統(tǒng)的原理和實(shí)現(xiàn)應(yīng)用分析

1、 前言

不得不承認(rèn)的一件事情是，在SLAM的這個(gè)技術(shù)領(lǐng)域內(nèi)各位大佬們的內(nèi)卷可以說(shuō)是越來(lái)越嚴(yán)重了，每一次在拜讀了行業(yè)內(nèi)頂尖的論文或者是觀看了行業(yè)大佬們發(fā)出來(lái)的技術(shù)demo視頻后，我本人就會(huì)陷入無(wú)限的自我懷疑當(dāng)中……

精度、魯棒性、效率等等問(wèn)題大家好像在各自的數(shù)據(jù)集中都實(shí)現(xiàn)了solved，而我卻還在我“目前的數(shù)據(jù)集”掙扎（文字狗頭）。摒棄掉妄自菲薄的念頭，針對(duì)之前的文章中各位大佬在life-long問(wèn)題中提出的勘誤點(diǎn)：

1） Life-long問(wèn)題中需要克服的問(wèn)題在于如何應(yīng)對(duì)視角變化，天氣變化，光照變化條件以及動(dòng)態(tài)障礙物環(huán)境下，確保定位和建模的精度。

2） Life-long實(shí)際是包含了兩個(gè)維度的問(wèn)題：long term和large scale。若是基于圖優(yōu)化的激光匹配算法，需要考慮怎么保證約束的準(zhǔn)確性，當(dāng)環(huán)境發(fā)生變化之后，匹配算法非常容易產(chǎn)生歧義，需要避免感知混淆的問(wèn)題。根據(jù)上述的問(wèn)題點(diǎn)，我們可以發(fā)現(xiàn)在life-long問(wèn)題解決的關(guān)鍵流程中都會(huì)出現(xiàn)環(huán)境變換檢測(cè)的功能，這個(gè)功能既需要包括針對(duì)短期的高動(dòng)態(tài)障礙物的檢測(cè)，也需要包括針對(duì)長(zhǎng)期的緩慢環(huán)境變化的檢測(cè)。而針對(duì)這個(gè)關(guān)鍵的環(huán)境變換檢測(cè)功能，學(xué)術(shù)界也提出了很多種方案，本片文章則將簡(jiǎn)單分析一下韓國(guó)KAIST Department of Civil and Environmental Engineering 在2021年發(fā)表的“LT-mapper： A Modular Framework for LiDAR-based Lifelong Mapping“論文。

2、 LT-mapper系統(tǒng)總概

圖：LT-Mapper系統(tǒng)的主要流程框圖由LT-Mapper系統(tǒng)的主要流程框圖，我們可以發(fā)現(xiàn)其系統(tǒng)主要由LT-SLAM， LT-removert， and LT-map三個(gè)模塊組成，各個(gè)模塊各司其職又互相獨(dú)立共同保證整個(gè)life long框架的正常運(yùn)行。

（1） LT-SLAM， 在LT-SLAM模塊中，Kim團(tuán)隊(duì)利用multi-session SLAM，基于激光雷達(dá)的global localizer實(shí)現(xiàn)回環(huán)檢測(cè)，以此內(nèi)部聯(lián)合優(yōu)化多個(gè)session SLAM的軌跡。在這個(gè)模塊中，query map被注冊(cè)添加到現(xiàn)有的central map（target map）。

（2） LT-removert， 該模塊用于處理在query map和central map（target map）對(duì)齊時(shí)的”模糊性”問(wèn)題，一方面用以去除map中的噪聲，一方面用以去除map中的高動(dòng)態(tài)物體點(diǎn)云（High Dynamic Points）

（3） LT-map，將query map注冊(cè)添加到現(xiàn)有的central map并移除HD點(diǎn)后，通過(guò)在query map和central map間進(jìn)行set difference operation的操作來(lái)檢測(cè)環(huán)境上的變化。此時(shí)檢測(cè)變化的time scale就會(huì)大于之前LT-removert模塊檢測(cè)高動(dòng)態(tài)物體點(diǎn)云的time scale了，即是探測(cè)不同session靜態(tài)地圖的變化，其階段檢測(cè)出來(lái)的動(dòng)態(tài)點(diǎn)云稱為低動(dòng)態(tài)點(diǎn)云（LD）。低動(dòng)態(tài)點(diǎn)云（LD）隨后將其進(jìn)一步分為“新出現(xiàn)“點(diǎn)（positive difference （PD））和”消失“點(diǎn)（negative difference （ND））兩類。

3、 LT-SLAM方法

該模塊本質(zhì)上就是multi-session SLAM中多SLAM Trajectories的拼接對(duì)齊問(wèn)題，論文中使用的匹配方案是ICP或者SCAN-CONTEXT（SC-Loops也是Kim團(tuán)隊(duì)研究和發(fā)表的一種回環(huán)檢測(cè)方法。論文可詳見(jiàn)鏈接：Scan Context： Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map）來(lái)計(jì)算過(guò)兩個(gè)點(diǎn)云“關(guān)鍵幀”之間的6自由度的變換關(guān)系。在通過(guò)回環(huán)檢測(cè)計(jì)算出frame之間的約束關(guān)系后，在pose-graph中引入相關(guān)的對(duì)象和約束進(jìn)行multiple sessions的優(yōu)化計(jì)算以實(shí)現(xiàn)軌跡間的對(duì)齊。值得說(shuō)明的是，論文中提出了anchor node（錨點(diǎn)）的概念，該anchor node用以替代SLAM中的key node（frame），因?yàn)長(zhǎng)T-SLAM中會(huì)處理許多不同時(shí)期的SLAM trajectories，若是以傳統(tǒng)意義上的關(guān)鍵幀作為一個(gè)處理單元的話，則計(jì)算量和復(fù)雜度太大了。

因此，anchor node（錨點(diǎn)）則代表了若干個(gè)key node（frame）形成的map的原點(diǎn)，在SLAM軌跡的優(yōu)化過(guò)程中也將優(yōu)化的處理單元改為了anchor node（類似cartographer中submap的概念）。而系統(tǒng)中使用anchor node，一方面減少了在回環(huán)檢測(cè)后pose-graph優(yōu)化調(diào)整的復(fù)雜度，另一方面也減少了系統(tǒng)地圖環(huán)境變化和更新的復(fù)雜度。

4、 LT-REMOVERT方法

回到對(duì)環(huán)境中動(dòng)態(tài)物體點(diǎn)云的處理和分類，文章將所有的動(dòng)態(tài)點(diǎn)分類成了兩類：HD（高變化的動(dòng)態(tài)障礙物）和LD（緩慢變化的場(chǎng)景點(diǎn)）。根據(jù)以上的分類，再將整個(gè)環(huán)境變化的檢測(cè)功能分成兩個(gè)sessions：

1. 首先刪除HD而不刪除LD點(diǎn)，在這個(gè)過(guò)程中文章是使用了REMOVERT算法作為HD的刪除引擎，具體的算法原理和流程可以詳見(jiàn)本人的另一篇文章SLAM動(dòng)態(tài)障礙物濾除 | IROS 2020 REMOVERT：Remove， then Revert的論文解析與實(shí)現(xiàn)結(jié)果。

2. 在完成對(duì)HD點(diǎn)的刪除后，進(jìn)入LD的檢測(cè)階段，該階段中會(huì)先將query map和target map的坐標(biāo)系對(duì)齊并且移除兩者中的HD后，為target map構(gòu)造了一個(gè)kd-tree并判斷query map中的點(diǎn)云在半徑r米的范圍內(nèi)是否有足夠多的target map點(diǎn)云，如果沒(méi)有，則該query map中的點(diǎn)為L(zhǎng)D。然后再對(duì)LD點(diǎn)進(jìn)行ND和PD的劃分。其中PD（Positive Difference）指的是在query map中新增加上的點(diǎn)云變化，ND（Negative Difference）則是指原先存在而現(xiàn)在query map中消失了的點(diǎn)云，所以根據(jù)上述的定義我們相應(yīng)需要去除的點(diǎn)云即為ND，需要添加的點(diǎn)云為PD。

而Kim在LD的基礎(chǔ)上進(jìn)行ND和PD的區(qū)分，主要是為了解決一種點(diǎn)云“誤殺”的問(wèn)題：Occlusions（遮擋），試想一種情況：target map中有一個(gè)靜態(tài)物體O并在完整構(gòu)建了其點(diǎn)云“形狀”，而在query map中實(shí)際的環(huán)境是該靜態(tài)物體O前突然多了一堵墻遮擋住了傳感器關(guān)于物體O的識(shí)別。

那么在query map中該物體O點(diǎn)云會(huì)被先識(shí)別為L(zhǎng)D，但進(jìn)一步的分析是該物體O的點(diǎn)云是應(yīng)當(dāng)保留而非去除的，Kim稱此類被遮擋住的點(diǎn)云為weak ND，并且weak ND是不會(huì)被去除以避免“誤殺”的情況。對(duì)于這一步關(guān)于“Weak ND Preservation”的問(wèn)題，Kim團(tuán)隊(duì)再次使用了Removert，但進(jìn)行了修改：不同于原來(lái)的Removert只刪除視點(diǎn)原點(diǎn)近處的地圖點(diǎn)，而修改版本的Removert則關(guān)注于刪除原始ND map中的遠(yuǎn)處的點(diǎn)，并然后將它們還原到static map中。

說(shuō)完這么多，我們參照論文中的實(shí)例進(jìn)行一下說(shuō)明，下圖A、B分別為同一街道不同時(shí)期的地圖（MulRan dataset DCC 01和DCC 02），其關(guān)鍵的不同之處在于馬路邊上是否存在墻面。而且在有墻面的地圖后面是不存在點(diǎn)云的（因?yàn)檎趽醯脑颍?/p>

圖A：MulRan dataset DCC 01（有墻面）

圖B：MulRan dataset DCC 02（無(wú)墻面）

具體步驟的結(jié)果可詳見(jiàn)下圖，其中標(biāo)記為藍(lán)色的點(diǎn)云為PD，標(biāo)記為紅色的點(diǎn)云為ND，灰色的點(diǎn)云為weak ND。情況（1）：選擇A為target map，B為query map，左列的環(huán)境檢測(cè)結(jié)果圖片是普通的kd-tree取半徑集合差操作的獲取的na?ve ND和PD點(diǎn)，右欄則顯示了在檢測(cè)處LD后進(jìn)一步PD和ND區(qū)分的結(jié)果。在Case 1中，紅色ND點(diǎn)大部分被刪除，而添加了整個(gè)藍(lán)色PD點(diǎn)（相對(duì)于A，B中新增了許多點(diǎn)云，所以為PD）。

圖：情況1下的環(huán)境檢測(cè)結(jié)果情況（2）：選擇B為target map，A為query map，在map A中的部分地面點(diǎn)被遮擋，并在左欄中錯(cuò)誤地標(biāo)記為ND。在右欄中，只有強(qiáng)ND（紅色）點(diǎn)被移除，弱ND點(diǎn)被還原（即灰色標(biāo)記的地面點(diǎn)）。由此可見(jiàn)，Kim團(tuán)結(jié)針對(duì)LD點(diǎn)云的進(jìn)一步PD、ND 和weak ND區(qū)分是可以有效避免遮擋引起的誤殺問(wèn)題的。

圖：情況2下的環(huán)境檢測(cè)結(jié)果

5、 LT-MAP方法

該部分的主要任務(wù)就是地圖的更新（map update）和地圖的長(zhǎng)期管理（Long-term Map Management）。

針對(duì)地圖的更新（map update），論文中使用了一個(gè)公式進(jìn)行了表述，該公式就是LT-map部分最核心的邏輯：

其中為去除了HD關(guān)鍵幀點(diǎn)云，函數(shù)ND（*）和PD（*）表示返回在關(guān)鍵幀中類型為ND 和 PD的點(diǎn)云，一言以蔽之，給到central map和query map，去除HD和ND的點(diǎn)云并保證PD點(diǎn)云保留。

針對(duì)地圖的長(zhǎng)期管理（Long-term Map Management），在論文中的進(jìn)一步測(cè)試說(shuō)明了LT-mapper在更新world representation時(shí)分為兩種方法，如下圖所示。第一，LT-mapper可以高效地通過(guò)只發(fā)送LD類型的環(huán)境變化點(diǎn)云（而不是整個(gè)關(guān)鍵幀點(diǎn)云地圖），以更改到一個(gè)central server來(lái)維護(hù)一個(gè)live map。第二個(gè)表述為meta map（區(qū)別于live map）， LT-mapper在更新擴(kuò)展時(shí)沒(méi)有考慮添加weak PD類型的點(diǎn)云，即不考慮短期靜止或周期性的變化的點(diǎn)云。

圖：地圖的長(zhǎng)期管理的兩種形式

6、 LT-MAPPER關(guān)鍵代碼實(shí)現(xiàn)方法

第一， 先上LT-Mapper的GitHub開(kāi)源代碼鏈接：https://github.com/gisbi-kim/lt-mapper該開(kāi)源代碼整體流程和函數(shù)的設(shè)計(jì)都比較清晰，較為晦澀的地方可能也就是LT-REMOVERT模塊了，本節(jié)也將根據(jù)系統(tǒng)框圖的順序從LT-SLAM， LT-removert， and LT-map各個(gè)模塊進(jìn)行關(guān)鍵部分代碼的展示。

6.1 LT-SLAM

該部分和其他SLAM的回環(huán)檢測(cè)和因子圖優(yōu)化的過(guò)程實(shí)現(xiàn)并沒(méi)有本質(zhì)上的區(qū)別，僅僅是該模塊是直接加載Single session SLAM的軌跡和因子圖信息。在multiple sessions間進(jìn)行回環(huán)檢測(cè)的操作，并在計(jì)算出約束后添加到已有的因子圖中完成優(yōu)化匹配以實(shí)現(xiàn)multipleSessions SLAM 軌跡信息的對(duì)齊拼接。

1、void LTslam::run( void )

6.2 LT-removert

整個(gè)Removerter：：run（void）的流程分為了（1）初始化加載地圖和預(yù)處理，（2）高動(dòng)態(tài)點(diǎn)云HD的去除，（3）低動(dòng)態(tài)點(diǎn)云LD的檢測(cè)，（4）LT-map。代碼詳見(jiàn)如下：

上述代碼中，extractLowDynPointsViaKnnDiff（_target_map）是使用了pcl庫(kù)中KdTreeFLANN的結(jié)構(gòu)進(jìn)行LowDynamicPoints的檢測(cè)并儲(chǔ)存。constructGlobalNDMap（）函數(shù)是基于Session類中的坐標(biāo)和對(duì)于點(diǎn)云數(shù)據(jù)進(jìn)行點(diǎn)云拼接生成point cloud map，filterStrongND（）和removeWeakNDMapPointsHavingStrongNDInNear（）與函數(shù)名的定義符合，就是為了將strong ND獨(dú)立的返回出來(lái)以便在LT-map中使用。

6.3 LT-map

在整個(gè)代碼工程中，LT-map和LT-removert都放在了`ltremovert`的文件目錄下，和論文中的說(shuō)明對(duì)應(yīng)：LT-map中在session間環(huán)境變化的檢測(cè)方法是基于LT-removert進(jìn)行的修改。removert是用以進(jìn)行single session中map的高動(dòng)態(tài)點(diǎn)云的清洗，LT-map則是實(shí)現(xiàn)sessions間“靜態(tài)”環(huán)境物體的變化檢測(cè)。在6.2中的Removerter：：run（void）的流程中，我們可以看到在完成了地圖加載初始化、高動(dòng)態(tài)點(diǎn)云HD的去除和低動(dòng)態(tài)點(diǎn)云LD的檢測(cè)后，就進(jìn)入了LT-map的地圖更新處理了。

updateCurrentMap()函數(shù)的功能就是將query sess map和central sess map對(duì)齊整合到一起，并保證去除了strong ND points和保留weak ND points以及添加strong PD points到整合對(duì)齊后的地圖中。