不知道有沒有做激光SLAM的小伙伴呢短蜕？在激光SLAM中傻咖，相關(guān)性掃描匹配可是重點(diǎn)中的重點(diǎn)，而分支限界加速更是大大提高了它的實(shí)時性卿操。本文就來詳細(xì)解讀一下其中的奧秘。
激光SLAM前端
既然是SLAM扇雕，那么通常都會分為前端和后端窥摄。前端充當(dāng)里程計的角色，比如我們熟知的VO（視覺里程計）和VIO（視覺慣性里程計）哨苛。激光SLAM也需要前端，對于每一幀點(diǎn)云建峭，通常是使用所謂的掃描匹配（scan-match）方法進(jìn)行位姿估計。
簡單地說使碾，掃描匹配就是想辦法把兩幀點(diǎn)云對齊祝懂，對齊過程中的旋轉(zhuǎn)和平移就是兩幀之間的相對位姿。這是最最簡單的方式砚蓬，實(shí)際中，為了提高魯棒性祟剔，通常把當(dāng)前幀點(diǎn)云和地圖進(jìn)行掃描匹配摩梧，更不容易出錯。
掃描匹配的實(shí)現(xiàn)方式有兩種仅父，第一種是相關(guān)性掃描匹配笙纤，第二種是基于優(yōu)化的掃描匹配。第一種思路簡單省容，但有一些復(fù)雜的細(xì)節(jié)，而第二種則是構(gòu)建非線性最小二乘優(yōu)化問題并求解阿宅。實(shí)際中寞酿，通常是兩種方法同時用脱柱，先用相關(guān)性掃描匹配求初值，再用基于優(yōu)化的掃描匹配進(jìn)一步優(yōu)化。
相關(guān)性掃描匹配
相關(guān)性掃描匹配的思路其實(shí)非常簡單煌茴。想象給定一個柵格地圖日川，再給定當(dāng)前幀的點(diǎn)云，怎樣才能知道激光雷達(dá)所在的位置呢龄句？最簡單的辦法，把激光雷達(dá)放在地圖的每個格子上傀蓉，看在這個位置時职抡，點(diǎn)云是否與地圖重合，重合程度最高的位置就是激光雷達(dá)的真實(shí)位姿谱净。
顯然擅威，這個方法就是暴力搜索。雖然肯定搜得出來郊丛，但速度比較慢。想要加速捻艳，就得祭出我們的算法神器了——分支限界庆猫。
分支限界加速
如果上過計算機(jī)算法課，就一定接觸過分支限界法月培。這是一個通用的算法，一般用于搜索離散空間的最優(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)的位姿岖免，實(shí)際上是把位姿放在格子的中心成翩，計算其點(diǎn)云的評分（即hit點(diǎn)的評分之和赦役，下文直接稱其為位姿的評分）。但是格子中心的位姿并不能代表格子其它位置的位姿掂摔，很有可能格子中心的評分低于其它位置的評分乙漓，那么第二層最優(yōu)的格子中并不一定包含第三層最優(yōu)的格子。怎么辦呢叭披，我們可以想辦法讓格子中心的評分高于其所有子格子的評分，此時格子的評分是其子格子的評分上界嚼贡，這樣就可以保證子格子的最高評分體現(xiàn)在父格子中同诫。
但是，此時又引入了第二個問題叮盘，雖然格子的評分是其子格子的評分上界霹俺，但并非上確界。也就是說丙唧，可能存在格子的評分很高，但其所有子格子的評分都很低的情況蝌戒。這樣的話沼琉，我們?nèi)匀粺o法選取最大評分格子的同時拋棄其它格子。
剩下的選擇就只有一個友鼻，絕不拋棄任何一個可能存在最優(yōu)解的格子，只拋棄那些絕對不存在最優(yōu)解的格子彩扔。這正是分支限界的思想。
具體實(shí)現(xiàn)的時候贾惦，先從根節(jié)點(diǎn)開始遍歷敦捧，把它拆分成4個子格子。這4個子格子分別計算評分兢卵，并由高到低排序秽荤。從中選出分?jǐn)?shù)最高的格子，進(jìn)一步拆分成4個子格子窃款。假設(shè)這4個子格子已經(jīng)到了葉子節(jié)點(diǎn)，也就是達(dá)到了真實(shí)地圖的分辨率第喳。此時計算出的4個子格子的評分代表了真實(shí)的位姿評分（只有葉子節(jié)點(diǎn)的評分是真實(shí)評分踱稍，其它格子的評分都是上界），找出其最大值扩淀，記作best_score啤挎。以上過程，我們體會了分支是如何實(shí)現(xiàn)的庆聘。接下來，該輪到限界出場了象对。第二層的格子還有3個未曾探索宴抚，我們當(dāng)然是選擇最大的那個開始分支甫煞。但別急冠绢，先看一下它的評分是否大于best_score，如果是楷力，繼續(xù)分支邮利，如果否垃帅，就可以直接剪枝了，拋棄這個格子及其所有子格子方庭。因?yàn)楦褡拥脑u分代表了其子格子評分的上界酱固，如果上界都小于best_score，就不可能再有子格子的評分大于它了运悲。按照如此方法班眯，大刀闊斧地剪枝即可。注意署隘，當(dāng)遇到評分大于best_score的葉子節(jié)點(diǎn)時，記得更新best_score违崇，這樣可以更快地縮小搜索空間诊霹。
講到這里，你是不是有點(diǎn)摸不著頭腦肴楷？別怕荠呐，因?yàn)檫@里有個最最關(guān)鍵的問題我還沒有解釋砂客，也就是如何能夠快速計算出格子評分的上界呵恢。
計算評分上界
先說一個平凡方法。既然要算子格子評分的上界彤恶，那就把每一個子格子拿出來算一遍鳄橘，求個最大值。對了瘫怜，子格子還會有子格子鲸湃，所以這是個遞歸運(yùn)算，直到把葉子節(jié)點(diǎn)都算出來才行暗挑。那這跟直接暴力搜索就沒什么兩樣了炸裆，不行不行。
我們還是得追求在O(1)時間復(fù)雜度內(nèi)求出格子的評分烹看。想了想，之所以葉子節(jié)點(diǎn)評分可以在O(1)時間內(nèi)求出贝奇，是因?yàn)樗侵苯釉诘貓D中查表得到的（嚴(yán)謹(jǐn)?shù)卣f靠胜，是通過查其所有hit點(diǎn)的評分再求和得到的，但由于hit點(diǎn)數(shù)量固定陕习，可以認(rèn)為時間復(fù)雜度是O(1)）址愿。但我們構(gòu)造的多分辨率地圖能不能也維護(hù)類似的概率表格，使得格子的評分可以在對應(yīng)分辨率地圖中查詢得到呢损合？
多么機(jī)智的想法啊跋炕！試想律适，把地圖中每個格子的概率用其附近區(qū)域內(nèi)的最大概率代替。這樣的話捂贿，直接查對應(yīng)分辨率地圖就可以得到格子的評分，因?yàn)檫@個評分已經(jīng)事先在附近區(qū)域中取了最大值扣草。
上面提到的“某個范圍”吁系，需要與地圖分辨率保持一致白魂。也就是說，越低分辨率的地圖（也就是越高層的地圖）蕴坪，應(yīng)該在越大的區(qū)域求最大值敬锐，其大小與當(dāng)前分辨率下格子的大小一致。
不同分辨率的地圖需要事先計算好径玖，大概長這個樣子：
可以看到颤介，越靠后的地圖分辨率越低，格子中的概率越接近1冤灾，也就代表了越高的上限辕近。這與我們的直觀理解是一致的。
總結(jié)
說實(shí)話归粉，本文內(nèi)容非常抽象椿疗，又限于筆者畫圖能力太差，實(shí)在畫不出合適的插圖糠悼，只能靠讀者自己想象了变丧。
不得不說，除非事先有所了解绢掰，否則讀了之后可能還是一頭霧水痒蓬。所以，如果你對激光SLAM還沒有整體的了解滴劲，還不知道什么是點(diǎn)云攻晒、什么是地圖、如何計算概率班挖。那么請關(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
