不知道有沒有做激光SLAM的小伙伴呢？在激光SLAM中，相關(guān)性掃描匹配可是重點中的重點，而分支限界加速更是大大提高了它的實時性。本文就來詳細(xì)解讀一下其中的奧秘。

激光SLAM前端

既然是SLAM，那么通常都會分為前端和后端。前端充當(dāng)里程計的角色，比如我們熟知的VO（視覺里程計）和VIO（視覺慣性里程計）。激光SLAM也需要前端，對于每一幀點云，通常是使用所謂的掃描匹配（scan-match）方法進行位姿估計。

簡單地說，掃描匹配就是想辦法把兩幀點云對齊，對齊過程中的旋轉(zhuǎn)和平移就是兩幀之間的相對位姿。這是最最簡單的方式，實際中，為了提高魯棒性，通常把當(dāng)前幀點云和地圖進行掃描匹配，更不容易出錯。

掃描匹配的實現(xiàn)方式有兩種，第一種是相關(guān)性掃描匹配，第二種是基于優(yōu)化的掃描匹配。第一種思路簡單，但有一些復(fù)雜的細(xì)節(jié)，而第二種則是構(gòu)建非線性最小二乘優(yōu)化問題并求解。實際中，通常是兩種方法同時用，先用相關(guān)性掃描匹配求初值，再用基于優(yōu)化的掃描匹配進一步優(yōu)化。

相關(guān)性掃描匹配

相關(guān)性掃描匹配的思路其實非常簡單。想象給定一個柵格地圖，再給定當(dāng)前幀的點云，怎樣才能知道激光雷達所在的位置呢？最簡單的辦法，把激光雷達放在地圖的每個格子上，看在這個位置時，點云是否與地圖重合，重合程度最高的位置就是激光雷達的真實位姿。

顯然，這個方法就是暴力搜索。雖然肯定搜得出來，但速度比較慢。想要加速，就得祭出我們的算法神器了——分支限界。

分支限界加速

如果上過計算機算法課，就一定接觸過分支限界法。這是一個通用的算法，一般用于搜索離散空間的最優(yōu)解。我們這里的解空間顯然就是一個離散的空間，而且可以很方便地按照地圖分辨率構(gòu)造樹形結(jié)構(gòu)。

舉例來說，假設(shè)有一張4×4的地圖，我們降低其分辨率，每2×2的格子合并成一個，得到一張2×2的地圖，再降低其分辨率，得到一張1×1的地圖。這樣，地圖被分成了三層，如下圖所示。

多分辨率地圖

有了多分辨率地圖后，我們先在第二層的解空間中搜索，找到最優(yōu)的位姿對應(yīng)的格子。然后再在該格子中搜索下一層中對應(yīng)的小格子，找到最優(yōu)的位姿。

這么簡單？當(dāng)然不是。請注意，上面的做法是完全錯誤的。它有兩個問題。首先，在第二層的格子中搜索最優(yōu)的位姿，實際上是把位姿放在格子的中心，計算其點云的評分（即hit點的評分之和，下文直接稱其為位姿的評分）。但是格子中心的位姿并不能代表格子其它位置的位姿，很有可能格子中心的評分低于其它位置的評分，那么第二層最優(yōu)的格子中并不一定包含第三層最優(yōu)的格子。怎么辦呢，我們可以想辦法讓格子中心的評分高于其所有子格子的評分，此時格子的評分是其子格子的評分上界，這樣就可以保證子格子的最高評分體現(xiàn)在父格子中。

但是，此時又引入了第二個問題，雖然格子的評分是其子格子的評分上界，但并非上確界。也就是說，可能存在格子的評分很高，但其所有子格子的評分都很低的情況。這樣的話，我們?nèi)匀粺o法選取最大評分格子的同時拋棄其它格子。

剩下的選擇就只有一個，絕不拋棄任何一個可能存在最優(yōu)解的格子，只拋棄那些絕對不存在最優(yōu)解的格子。這正是分支限界的思想。

具體實現(xiàn)的時候，先從根節(jié)點開始遍歷，把它拆分成4個子格子。這4個子格子分別計算評分，并由高到低排序。從中選出分?jǐn)?shù)最高的格子，進一步拆分成4個子格子。假設(shè)這4個子格子已經(jīng)到了葉子節(jié)點，也就是達到了真實地圖的分辨率。此時計算出的4個子格子的評分代表了真實的位姿評分（只有葉子節(jié)點的評分是真實評分，其它格子的評分都是上界），找出其最大值，記作best_score。以上過程，我們體會了分支是如何實現(xiàn)的。接下來，該輪到限界出場了。第二層的格子還有3個未曾探索，我們當(dāng)然是選擇最大的那個開始分支。但別急，先看一下它的評分是否大于best_score，如果是，繼續(xù)分支，如果否，就可以直接剪枝了，拋棄這個格子及其所有子格子。因為格子的評分代表了其子格子評分的上界，如果上界都小于best_score，就不可能再有子格子的評分大于它了。按照如此方法，大刀闊斧地剪枝即可。注意，當(dāng)遇到評分大于best_score的葉子節(jié)點時，記得更新best_score，這樣可以更快地縮小搜索空間。

講到這里，你是不是有點摸不著頭腦？別怕，因為這里有個最最關(guān)鍵的問題我還沒有解釋，也就是如何能夠快速計算出格子評分的上界。

計算評分上界

先說一個平凡方法。既然要算子格子評分的上界，那就把每一個子格子拿出來算一遍，求個最大值。對了，子格子還會有子格子，所以這是個遞歸運算，直到把葉子節(jié)點都算出來才行。那這跟直接暴力搜索就沒什么兩樣了，不行不行。

我們還是得追求在O(1)時間復(fù)雜度內(nèi)求出格子的評分。想了想，之所以葉子節(jié)點評分可以在O(1)時間內(nèi)求出，是因為它是直接在地圖中查表得到的（嚴(yán)謹(jǐn)?shù)卣f，是通過查其所有hit點的評分再求和得到的，但由于hit點數(shù)量固定，可以認(rèn)為時間復(fù)雜度是O(1)）。但我們構(gòu)造的多分辨率地圖能不能也維護類似的概率表格，使得格子的評分可以在對應(yīng)分辨率地圖中查詢得到呢？

多么機智的想法??！試想，把地圖中每個格子的概率用其附近區(qū)域內(nèi)的最大概率代替。這樣的話，直接查對應(yīng)分辨率地圖就可以得到格子的評分，因為這個評分已經(jīng)事先在附近區(qū)域中取了最大值。

上面提到的“某個范圍”，需要與地圖分辨率保持一致。也就是說，越低分辨率的地圖（也就是越高層的地圖），應(yīng)該在越大的區(qū)域求最大值，其大小與當(dāng)前分辨率下格子的大小一致。

不同分辨率的地圖需要事先計算好，大概長這個樣子：

可以看到，越靠后的地圖分辨率越低，格子中的概率越接近1，也就代表了越高的上限。這與我們的直觀理解是一致的。

總結(jié)

說實話，本文內(nèi)容非常抽象，又限于筆者畫圖能力太差，實在畫不出合適的插圖，只能靠讀者自己想象了。

不得不說，除非事先有所了解，否則讀了之后可能還是一頭霧水。所以，如果你對激光SLAM還沒有整體的了解，還不知道什么是點云、什么是地圖、如何計算概率。那么請關(guān)注一下文末的參考資料，相信會對你有所幫助。

相關(guān)性掃描匹配及分枝限界加速在谷歌Cartographer的論文中有詳細(xì)介紹，建議大家去看一看。

參考資料

激光SLAM的前端配準(zhǔn)方法 曾書格
Real-Time Loop Closure in 2D LIDAR SLAM Wolfgang Hess, Damon Kohler, Holger Rapp, Daniel Andor