相關(guān)性掃描匹配之分支限界加速
激光SLAM前端
既然是SLAM，那么通常都會(huì)分為前端和后端俱济。前端充當(dāng)里程計(jì)的角色，比如我們熟知的VO（視覺(jué)里程計(jì)）和VIO（視覺(jué)慣性里程計(jì)）。激光SLAM也需要前端饱搏，對(duì)于每一幀點(diǎn)云，通常是使用所謂的掃描匹配（scan-match）方法進(jìn)行位姿估計(jì)置逻。
簡(jiǎn)單地說(shuō)推沸，掃描匹配就是想辦法把兩幀點(diǎn)云對(duì)齊，對(duì)齊過(guò)程中的旋轉(zhuǎn)和平移就是兩幀之間的相對(duì)位姿券坞。這是最最簡(jiǎn)單的方式鬓催，實(shí)際中，為了提高魯棒性恨锚，通常把當(dāng)前幀點(diǎn)云和地圖進(jìn)行掃描匹配宇驾，更不容易出錯(cuò)。
掃描匹配的實(shí)現(xiàn)方式有兩種猴伶，第一種是相關(guān)性掃描匹配课舍，第二種是基于優(yōu)化的掃描匹配。第一種思路簡(jiǎn)單他挎，但有一些復(fù)雜的細(xì)節(jié)筝尾，而第二種則是構(gòu)建非線性最小二乘優(yōu)化問(wèn)題并求解。實(shí)際中办桨，通常是兩種方法同時(shí)用筹淫，先用相關(guān)性掃描匹配求初值，再用基于優(yōu)化的掃描匹配進(jìn)一步優(yōu)化呢撞。
相關(guān)性掃描匹配
相關(guān)性掃描匹配的思路其實(shí)非常簡(jiǎn)單损姜。想象給定一個(gè)柵格地圖，再給定當(dāng)前幀的點(diǎn)云殊霞，怎樣才能知道激光雷達(dá)所在的位置呢薛匪？最簡(jiǎn)單的辦法，把激光雷達(dá)放在地圖的每個(gè)格子上脓鹃，看在這個(gè)位置時(shí)逸尖，點(diǎn)云是否與地圖重合，重合程度最高的位置就是激光雷達(dá)的真實(shí)位姿瘸右。
顯然娇跟，這個(gè)方法就是暴力搜索。雖然肯定搜得出來(lái)太颤，但速度比較慢苞俘。想要加速，就得祭出我們的算法神器了——分支限界龄章。
分支限界加速
如果上過(guò)計(jì)算機(jī)算法課吃谣，就一定接觸過(guò)分支限界法乞封。這是一個(gè)通用的算法，一般用于搜索離散空間的最優(yōu)解岗憋。我們這里的解空間顯然就是一個(gè)離散的空間肃晚，而且可以很方便地按照地圖分辨率構(gòu)造樹(shù)形結(jié)構(gòu)。
舉例來(lái)說(shuō)仔戈，假設(shè)有一張4×4的地圖关串，我們降低其分辨率，每2×2的格子合并成一個(gè)监徘，得到一張2×2的地圖晋修，再降低其分辨率，得到一張1×1的地圖凰盔。這樣墓卦，地圖被分成了三層，如下圖所示户敬。
多分辨率地圖
有了多分辨率地圖后落剪，我們先在第二層的解空間中搜索，找到最優(yōu)的位姿對(duì)應(yīng)的格子山叮。然后再在該格子中搜索下一層中對(duì)應(yīng)的小格子，找到最優(yōu)的位姿添履。
這么簡(jiǎn)單屁倔？當(dāng)然不是。請(qǐng)注意暮胧，上面的做法是完全錯(cuò)誤的锐借。它有兩個(gè)問(wèn)題。首先往衷，在第二層的格子中搜索最優(yōu)的位姿钞翔，實(shí)際上是把位姿放在格子的中心，計(jì)算其點(diǎn)云的評(píng)分（即hit點(diǎn)的評(píng)分之和席舍，下文直接稱(chēng)其為位姿的評(píng)分）布轿。但是格子中心的位姿并不能代表格子其它位置的位姿，很有可能格子中心的評(píng)分低于其它位置的評(píng)分来颤，那么第二層最優(yōu)的格子中并不一定包含第三層最優(yōu)的格子汰扭。怎么辦呢，我們可以想辦法讓格子中心的評(píng)分高于其所有子格子的評(píng)分福铅，此時(shí)格子的評(píng)分是其子格子的評(píng)分上界萝毛，這樣就可以保證子格子的最高評(píng)分體現(xiàn)在父格子中。
但是滑黔，此時(shí)又引入了第二個(gè)問(wèn)題笆包，雖然格子的評(píng)分是其子格子的評(píng)分上界环揽，但并非上確界。也就是說(shuō)庵佣，可能存在格子的評(píng)分很高歉胶，但其所有子格子的評(píng)分都很低的情況。這樣的話秧了，我們?nèi)匀粺o(wú)法選取最大評(píng)分格子的同時(shí)拋棄其它格子跨扮。
剩下的選擇就只有一個(gè)，絕不拋棄任何一個(gè)可能存在最優(yōu)解的格子验毡，只拋棄那些絕對(duì)不存在最優(yōu)解的格子衡创。這正是分支限界的思想。
具體實(shí)現(xiàn)的時(shí)候晶通，先從根節(jié)點(diǎn)開(kāi)始遍歷璃氢，把它拆分成4個(gè)子格子。這4個(gè)子格子分別計(jì)算評(píng)分狮辽，并由高到低排序一也。從中選出分?jǐn)?shù)最高的格子，進(jìn)一步拆分成4個(gè)子格子喉脖。假設(shè)這4個(gè)子格子已經(jīng)到了葉子節(jié)點(diǎn)椰苟，也就是達(dá)到了真實(shí)地圖的分辨率。此時(shí)計(jì)算出的4個(gè)子格子的評(píng)分代表了真實(shí)的位姿評(píng)分（只有葉子節(jié)點(diǎn)的評(píng)分是真實(shí)評(píng)分树叽，其它格子的評(píng)分都是上界）舆蝴，找出其最大值，記作best_score题诵。以上過(guò)程洁仗，我們體會(huì)了分支是如何實(shí)現(xiàn)的。接下來(lái)性锭，該輪到限界出場(chǎng)了赠潦。第二層的格子還有3個(gè)未曾探索，我們當(dāng)然是選擇最大的那個(gè)開(kāi)始分支草冈。但別急她奥，先看一下它的評(píng)分是否大于best_score，如果是怎棱，繼續(xù)分支方淤，如果否，就可以直接剪枝了蹄殃，拋棄這個(gè)格子及其所有子格子携茂。因?yàn)楦褡拥脑u(píng)分代表了其子格子評(píng)分的上界，如果上界都小于best_score诅岩，就不可能再有子格子的評(píng)分大于它了讳苦。按照如此方法带膜，大刀闊斧地剪枝即可。注意鸳谜，當(dāng)遇到評(píng)分大于best_score的葉子節(jié)點(diǎn)時(shí)膝藕，記得更新best_score，這樣可以更快地縮小搜索空間咐扭。
講到這里芭挽，你是不是有點(diǎn)摸不著頭腦？別怕蝗肪，因?yàn)檫@里有個(gè)最最關(guān)鍵的問(wèn)題我還沒(méi)有解釋?zhuān)簿褪侨绾文軌蚩焖儆?jì)算出格子評(píng)分的上界袜爪。
計(jì)算評(píng)分上界
先說(shuō)一個(gè)平凡方法。既然要算子格子評(píng)分的上界薛闪，那就把每一個(gè)子格子拿出來(lái)算一遍辛馆，求個(gè)最大值。對(duì)了豁延，子格子還會(huì)有子格子昙篙，所以這是個(gè)遞歸運(yùn)算，直到把葉子節(jié)點(diǎn)都算出來(lái)才行诱咏。那這跟直接暴力搜索就沒(méi)什么兩樣了苔可，不行不行。
我們還是得追求在O(1)時(shí)間復(fù)雜度內(nèi)求出格子的評(píng)分袋狞。想了想焚辅，之所以葉子節(jié)點(diǎn)評(píng)分可以在O(1)時(shí)間內(nèi)求出，是因?yàn)樗侵苯釉诘貓D中查表得到的（嚴(yán)謹(jǐn)?shù)卣f(shuō)硕并，是通過(guò)查其所有hit點(diǎn)的評(píng)分再求和得到的法焰，但由于hit點(diǎn)數(shù)量固定秧荆，可以認(rèn)為時(shí)間復(fù)雜度是O(1)）倔毙。但我們構(gòu)造的多分辨率地圖能不能也維護(hù)類(lèi)似的概率表格，使得格子的評(píng)分可以在對(duì)應(yīng)分辨率地圖中查詢得到呢乙濒？
多么機(jī)智的想法吧略摺！試想颁股，把地圖中每個(gè)格子的概率用其附近區(qū)域內(nèi)的最大概率代替么库。這樣的話，直接查對(duì)應(yīng)分辨率地圖就可以得到格子的評(píng)分甘有，因?yàn)檫@個(gè)評(píng)分已經(jīng)事先在附近區(qū)域中取了最大值诉儒。
上面提到的“某個(gè)范圍”，需要與地圖分辨率保持一致亏掀。也就是說(shuō)忱反，越低分辨率的地圖（也就是越高層的地圖）泛释，應(yīng)該在越大的區(qū)域求最大值，其大小與當(dāng)前分辨率下格子的大小一致温算。
不同分辨率的地圖需要事先計(jì)算好怜校，大概長(zhǎng)這個(gè)樣子：
可以看到膜钓，越靠后的地圖分辨率越低争拐，格子中的概率越接近1，也就代表了越高的上限碉输。這與我們的直觀理解是一致的巩割。
總結(jié)
說(shuō)實(shí)話裙顽，本文內(nèi)容非常抽象，又限于筆者畫(huà)圖能力太差喂分，實(shí)在畫(huà)不出合適的插圖锦庸，只能靠讀者自己想象了。
不得不說(shuō)蒲祈，除非事先有所了解甘萧，否則讀了之后可能還是一頭霧水。所以梆掸，如果你對(duì)激光SLAM還沒(méi)有整體的了解扬卷，還不知道什么是點(diǎn)云、什么是地圖酸钦、如何計(jì)算概率怪得。那么請(qǐng)關(guān)注一下文末的參考資料，相信會(huì)對(duì)你有所幫助卑硫。
相關(guān)性掃描匹配及分枝限界加速在谷歌Cartographer的論文中有詳細(xì)介紹徒恋，建議大家去看一看。
參考資料
激光SLAM的前端配準(zhǔn)方法 曾書(shū)格
