這兩天上網(wǎng)沖浪的時(shí)候看到有人說(shuō) Cartographer 里有一個(gè) PerformGlobalLocalization 函數(shù)用于全局重定位，可以看 cartographer github 倉(cāng)庫(kù)的?Issue #95 ?Localization in Existing Map。

不過(guò)我在源碼里搜了一下并沒(méi)有發(fā)現(xiàn)這個(gè)函數(shù)，然后又把舊版本的源碼也搜了一下，發(fā)現(xiàn)在0.3.0版本之后被刪除了，同時(shí)新版本中也提供了?pure_localization 模式，函數(shù)被刪除應(yīng)該和這個(gè)有點(diǎn)關(guān)系。

在 0.3.0 或更早的版本中能找到叫 fast_global_localizer.h/.cpp?的文件，PerformGlobalLocalization 函數(shù)就在里面，函數(shù)聲明如下:

這個(gè)函數(shù)共有4個(gè)輸入和3個(gè)輸出：

>?cutoff? ? ? ? ? ? ?最低的匹配有效分?jǐn)?shù)；

> voxel_filter? ??點(diǎn)云濾波器；

> matchers? ? ? ?FastCorrelativeScanMatcher 匹配器，需要匹配多少副 Submap 就傳入多少個(gè)；

> point_cloud? 等待匹配的點(diǎn)云；

< best_pose_estimate? ?匹配到的最佳位姿；

< best_score? ? ? ? ? ? ? ? ? 匹配結(jié)果的分?jǐn)?shù)；

< return? ? ? ? ? ? ? ? ? ? ? ? ? ?是否有匹配結(jié)果；

函數(shù)的實(shí)現(xiàn)部分非常簡(jiǎn)單，就是一種 sacn-to-map 的匹配方法，先把點(diǎn)云過(guò)一遍濾波后，再逐個(gè)調(diào)用傳入的 matcher 的 MatchFullSubmap 方法進(jìn)行匹配，最后得到匹配得分最高的位姿和分?jǐn)?shù)后返回。

PerformGlobalLocalization?函數(shù)存在一個(gè)很明顯的缺陷啊，那就是可靠性不高，如果剛好傳入的點(diǎn)云處在兩個(gè) Submap 的交界處，或者是多個(gè) Submap 中純?cè)谙嗨频慕Y(jié)構(gòu)，匹配的結(jié)果都有可能是錯(cuò)誤的。我想在新版本中移除這個(gè)函數(shù)也是考慮到了這些問(wèn)題，新版的?pure_localization 模式用 map-to-map 的方式來(lái)實(shí)現(xiàn)類似全局重定位的功能，雖然剛啟動(dòng)的時(shí)候要生成一定數(shù)量的 Submap 后才能觸發(fā)當(dāng)前軌跡與先驗(yàn)地圖的軌跡的?map-to-map 匹配，但可靠性必然是比 PerformGlobalLocalization? 要高得多的，不過(guò)也還是沒(méi)能做到100%可靠。

在新版本中抄一下這個(gè)函數(shù)用來(lái)加速開機(jī)的重定位速度應(yīng)該也是不錯(cuò)的，在地圖比較大的時(shí)候，這個(gè)函數(shù)計(jì)算量會(huì)很大，再做一些優(yōu)化可能會(huì)更好：

（一）并行化改造，這個(gè)函數(shù)就是很粗暴的遍歷每一個(gè) matcher 去調(diào)用函數(shù)做匹配，完全可以并行去執(zhí)行這個(gè)循環(huán)，最后再匯總結(jié)果取出分?jǐn)?shù)最高的位姿。

（二）在有先驗(yàn)位姿的情況下縮小搜索范圍，可以用一些簡(jiǎn)單的策略來(lái)提高匹配的可靠性，例如只對(duì)距離最近的三幅 Submap 做匹配，并且直接調(diào)用?FastCorrelativeScanMatcher 的 Match 函數(shù)，通過(guò)提供先驗(yàn)位姿和有限的搜索范圍來(lái)匹配點(diǎn)云。

（三）和別的全局重定位方法做互補(bǔ)和交叉驗(yàn)證，例如有AMCL的時(shí)候，可以用?PerformGlobalLocalization?先計(jì)算出一個(gè)位姿作為 AMCL 全局搜索的先驗(yàn)輸入，AMCL 得到結(jié)果后再對(duì)比兩個(gè)值是否接近，利用詞袋模型實(shí)現(xiàn)的視覺(jué)全局重定位也可以做類似的操作，提高匹配速度并交叉驗(yàn)證結(jié)果。