1、 前言

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

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

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

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

2、 LT-mapper系統總概

圖：LT-Mapper系統的主要流程框圖由LT-Mapper系統的主要流程框圖，我們可以發現其系統主要由LT-SLAM， LT-removert， and LT-map三個模塊組成，各個模塊各司其職又互相獨立共同保證整個life long框架的正常運行。

（1） LT-SLAM， 在LT-SLAM模塊中，Kim團隊利用multi-session SLAM，基于激光雷達的global localizer實現回環檢測，以此內部聯合優化多個session SLAM的軌跡。在這個模塊中，query map被注冊添加到現有的central map（target map）。

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

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

3、 LT-SLAM方法

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

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

4、 LT-REMOVERT方法

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

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

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

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

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

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

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

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

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

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

圖：情況2下的環境檢測結果

5、 LT-MAP方法

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

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

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

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

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

6、 LT-MAPPER關鍵代碼實現方法

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

6.1 LT-SLAM

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

1、void LTslam::run( void )

6.2 LT-removert

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

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

6.3 LT-map

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

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