1、 前言

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

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

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

2） Life-long實際是包含了兩個維度的問題：long term和large scale。若是基于圖優(yōu)化的激光匹配算法，需要考慮怎么保證約束的準(zhǔn)確性，當(dāng)環(huán)境發(fā)生變化之后，匹配算法非常容易產(chǎn)生歧義，需要避免感知混淆的問題。根據(jù)上述的問題點，我們可以發(fā)現(xiàn)在life-long問題解決的關(guān)鍵流程中都會出現(xiàn)環(huán)境變換檢測的功能，這個功能既需要包括針對短期的高動態(tài)障礙物的檢測，也需要包括針對長期的緩慢環(huán)境變化的檢測。而針對這個關(guān)鍵的環(huán)境變換檢測功能，學(xué)術(shù)界也提出了很多種方案，本片文章則將簡單分析一下韓國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三個模塊組成，各個模塊各司其職又互相獨立共同保證整個life long框架的正常運行。

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

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

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

3、 LT-SLAM方法

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

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

4、 LT-REMOVERT方法

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

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

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

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

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

說完這么多，我們參照論文中的實例進(jìn)行一下說明，下圖A、B分別為同一街道不同時期的地圖（MulRan dataset DCC 01和DCC 02），其關(guān)鍵的不同之處在于馬路邊上是否存在墻面。而且在有墻面的地圖后面是不存在點云的（因為遮擋的原因）。

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

圖B：MulRan dataset DCC 02（無墻面）

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

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

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

5、 LT-MAP方法

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

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

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

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

圖：地圖的長期管理的兩種形式

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

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

6.1 LT-SLAM

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

1、void LTslam::run( void )

6.2 LT-removert

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

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

6.3 LT-map

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

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