不知道有沒有做激光SLAM的小夥伴呢？在激光SLAM中，相關性掃描匹配可是重點中的重點，而分支限界加速更是大大提高了它的實時性。本文就來詳細解讀一下其中的奧祕。
激光SLAM前端
既然是SLAM，那麼通常都會分爲前端和後端。前端充當里程計的角色，比如我們熟知的VO（視覺里程計）和VIO（視覺慣性里程計）。激光SLAM也需要前端，對於每一幀點雲，通常是使用所謂的掃描匹配（scan-match）方法進行位姿估計。
簡單地說，掃描匹配就是想辦法把兩幀點雲對齊，對齊過程中的旋轉和平移就是兩幀之間的相對位姿。這是最最簡單的方式，實際中，爲了提高魯棒性，通常把當前幀點雲和地圖進行掃描匹配，更不容易出錯。
掃描匹配的實現方式有兩種，第一種是相關性掃描匹配，第二種是基於優化的掃描匹配。第一種思路簡單，但有一些複雜的細節，而第二種則是構建非線性最小二乘優化問題並求解。實際中，通常是兩種方法同時用，先用相關性掃描匹配求初值，再用基於優化的掃描匹配進一步優化。
相關性掃描匹配
相關性掃描匹配的思路其實非常簡單。想象給定一個柵格地圖，再給定當前幀的點雲，怎樣才能知道激光雷達所在的位置呢？最簡單的辦法，把激光雷達放在地圖的每個格子上，看在這個位置時，點雲是否與地圖重合，重合程度最高的位置就是激光雷達的真實位姿。
顯然，這個方法就是暴力搜索。雖然肯定搜得出來，但速度比較慢。想要加速，就得祭出我們的算法神器了——分支限界。
分支限界加速
如果上過計算機算法課，就一定接觸過分支限界法。這是一個通用的算法，一般用於搜索離散空間的最優解。我們這裏的解空間顯然就是一個離散的空間，而且可以很方便地按照地圖分辨率構造樹形結構。
舉例來說，假設有一張4×4的地圖，我們降低其分辨率，每2×2的格子合併成一個，得到一張2×2的地圖，再降低其分辨率，得到一張1×1的地圖。這樣，地圖被分成了三層，如下圖所示。
有了多分辨率地圖後，我們先在第二層的解空間中搜索，找到最優的位姿對應的格子。然後再在該格子中搜索下一層中對應的小格子，找到最優的位姿。
這麼簡單？當然不是。請注意，上面的做法是完全錯誤的。它有兩個問題。首先，在第二層的格子中搜索最優的位姿，實際上是把位姿放在格子的中心，計算其點雲的評分。但是格子中心的位姿並不能代表格子其它位置的位姿，很有可能格子中心的評分低於其它位置的評分，那麼第二層最優的格子中並不一定包含第三層最優的格子。怎麼辦呢，我們可以想辦法讓格子中心的評分高於其所有子格子的評分，此時格子的評分是其子格子的評分上界，這樣就可以保證子格子的最高評分體現在父格子中。
但是，此時又引入了第二個問題，雖然格子的評分是其子格子的評分上界，但並非上確界。也就是說，可能存在格子的評分很高，但其所有子格子的評分都很低的情況。這樣的話，我們仍然無法選取最大評分格子的同時拋棄其它格子。
剩下的選擇就只有一個，絕不拋棄任何一個可能存在最優解的格子，只拋棄那些絕對不存在最優解的格子。這正是分支限界的思想。
具體實現的時候，先從根節點開始遍歷，把它拆分成4個子格子。這4個子格子分別計算評分，並由高到低排序。從中選出分數最高的格子，進一步拆分成4個子格子。假設這4個子格子已經到了葉子節點，也就是達到了真實地圖的分辨率。此時計算出的4個子格子的評分代表了真實的位姿評分（只有葉子節點的評分是真實評分，其它格子的評分都是上界），找出其最大值，記作best_score。以上過程，我們體會了分支是如何實現的。接下來，該輪到限界出場了。第二層的格子還有3個未曾探索，我們當然是選擇最大的那個開始分支。但別急，先看一下它的評分是否大於best_score，如果是，繼續分支，如果否，就可以直接剪枝了，拋棄這個格子及其所有子格子。因爲格子的評分代表了其子格子評分的上界，如果上界都小於best_score，就不可能再有子格子的評分大於它了。按照如此方法，大刀闊斧地剪枝即可。注意，當遇到評分大於best_score的葉子節點時，記得更新best_score，這樣可以更快地縮小搜索空間。
講到這裏，你是不是有點摸不着頭腦？別怕，因爲這裏有個最最關鍵的問題我還沒有解釋，也就是如何能夠快速計算出格子評分的上界。
計算評分上界
先說一個平凡方法。既然要算子格子評分的上界，那就把每一個子格子拿出來算一遍，求個最大值。對了，子格子還會有子格子，所以這是個遞歸運算，直到把葉子節點都算出來才行。那這跟直接暴力搜索就沒什麼兩樣了，不行不行。
我們還是得追求在O(1)時間複雜度內求出格子的評分。想了想，之所以葉子節點評分可以在O(1)時間內求出，是因爲它是直接在地圖中查表得到的（請回憶，柵格地圖的每個格子保存了該格子被佔據的概率）。但我們構造的多分辨率地圖能不能也維護類似的概率表格，使得格子的評分可以在對應分辨率地圖中查詢得到呢？
多麼機智的想法啊！試想，把地圖中每個格子的概率用其附近區域內的最大概率代替。這樣的話，直接查對應分辨率地圖就可以得到格子的評分，因爲這個評分已經事先在附近區域中取了最大值。
上面提到的“某個範圍”，需要與地圖分辨率保持一致。也就是說，越低分辨率的地圖（也就是越高層的地圖），應該在越大的區域求最大值，其大小與當前分辨率下格子的大小一致。
不同分辨率的地圖需要事先計算好，大概長這個樣子：
可以看到，越靠後的地圖分辨率越低，格子中的概率越接近1，也就代表了越高的上限。這與我們的直觀理解是一致的。
總結
說實話，本文內容非常抽象，又限於筆者畫圖能力太差，實在畫不出合適的插圖，只能靠讀者自己想象了。
不得不說，除非事先有所瞭解，否則讀了之後可能還是一頭霧水。所以，如果你對激光SLAM還沒有整體的瞭解，還不知道什麼是點雲、什麼是地圖、如何計算概率。那麼請關注一下文末的參考資料，相信會對你有所幫助。
相關性掃描匹配及分枝限界加速在谷歌Cartographer的論文中有詳細介紹，建議大家去看一看。
參考資料
激光SLAM的前端配準方法 曾書格
Real-Time Loop Closure in 2D LIDAR SLAM Wolfgang Hess, Damon Kohler, Holger Rapp, Daniel Andor