給 Cartographer 添加全局重定位功能
新版本的 Cartographer 是沒有初始全局重定位的功能的，不過舊版本中有提供一個(gè)簡(jiǎn)單的實(shí)現(xiàn)參考，可以看我先前的文章《Cartographer 中的簡(jiǎn)易全局重定位方法》，但是舊版源碼中并沒有實(shí)際使用這個(gè)函數(shù)。其實(shí)這個(gè)函數(shù)是不夠用的，地圖大點(diǎn)都要匹配很久。
下面將會(huì)介紹如何在新版 carto 中添加上全局重定位功能，并提供接口供 cartographer_ros 調(diào)用。同時(shí)也會(huì)介紹如何用 carto 自帶的線程池加速重定位的計(jì)算。
可以先看看原版重定位函數(shù)的實(shí)現(xiàn)，我刪除了一些檢查和日志，只關(guān)注核心代碼：
可以看到，原版的實(shí)現(xiàn)真的很簡(jiǎn)單，就是拿一幀點(diǎn)云，對(duì)每一幅 Submap 都做一次全圖匹配。這里用的匹配器?FastCorrelativeScanMatcher?是 carto 用來做回環(huán)檢測(cè)的匹配器，匹配的成功率還是很不錯(cuò)的，不過匹配后還位姿還會(huì)差一點(diǎn)點(diǎn)才能對(duì)上，需要再調(diào)用一次?CeresScanMatcher2D?匹配器進(jìn)行精確匹配得到正確的位姿。
全局重定位功能實(shí)現(xiàn)過程
因?yàn)槿种囟ㄎ恍枰x取所有的 Submap 數(shù)據(jù)和最新的點(diǎn)云數(shù)據(jù)，所以把代碼加在?pose_graph_2d?模塊里是最方便的，cartographer_ros 里也能調(diào)用到。
首先就是接口的定義，cartographer_ros 里能通過?MapBuilderBridge?訪問到 carto 的?MapBuilder?實(shí)例，而?MapBuilder?又能訪問到?PoseGraph2D，所以我們只需要在?PoseGraphInterface?接口類中添加全局重定位的虛函數(shù)，然后在他的派生類中添加對(duì)應(yīng)的實(shí)現(xiàn)。
PoseGraphInterface?中添加的全局重定位的接口，我們的接口不需要傳入點(diǎn)云濾波器，內(nèi)部直接調(diào)體素濾波就行了。
然后到?PoseGraph2D?里添加對(duì)應(yīng)的函數(shù)。
到這里先介紹一下這個(gè)重定位函數(shù)的流程，然后逐個(gè)部分給出實(shí)現(xiàn)代碼。
創(chuàng)建線程池；
點(diǎn)云過一次體素濾波；
為每一個(gè) Submap 創(chuàng)建 FastCorrelativeScanMatcher 匹配器，在線程池中執(zhí)行，并等待所有匹配器創(chuàng)建完成；
遍歷每一個(gè)匹配器，調(diào)用 MatchFullSubmap 方法，記錄匹配結(jié)果和分?jǐn)?shù)，在線程池中執(zhí)行，并等待所有匹配完成；
找出匹配得分最高的位姿，創(chuàng)建 CeresScanMatcher2D 匹配器再次匹配得到準(zhǔn)確的位姿。
無(wú)論是創(chuàng)建匹配器還是全圖匹配，計(jì)算量都很大，使用線程池來執(zhí)行這些操作是必要的，我們直接用 carto 本身的線程池，線程池就用 CPU 核心數(shù)作為線程數(shù)量。
點(diǎn)云濾波也是必須的，一兩千個(gè)點(diǎn)云的計(jì)算量非常大，按 0.05 米的參數(shù)過一遍體素濾波后只剩幾百個(gè)點(diǎn)，匹配速度能提升一個(gè)數(shù)量級(jí)，計(jì)算結(jié)果幾乎也沒差別。
接著是為每一幅 Submap 創(chuàng)建匹配器，這一步需要在線程池里加速執(zhí)行。
匹配器創(chuàng)建好之后就是調(diào)用全圖匹配了，這一步同樣需要在線程池里執(zhí)行。
FastCorrelativeScanMatcher2D 匹配器找出可能性最高的位姿后，就是最后一步使用 CeresScanMatcher2D 匹配器去匹配精確位姿了。
該函數(shù)的使用方法也很簡(jiǎn)單，我的方法是純定位模式啟動(dòng)后，等加載完 pbstream 地圖文件和緩存有效一幀點(diǎn)云數(shù)據(jù)，通過?MapBuilderBridge?暴露的接口調(diào)用全局重定位函數(shù)，得到有效的匹配位姿后緩存起來，然后析構(gòu)掉當(dāng)前的 Node 實(shí)例，重新構(gòu)造一個(gè)并使用匹配到的位姿作為初始化位姿來重新開始純定位模式。
可以把這個(gè)接口封裝成一個(gè)RPC請(qǐng)求，grpc 或者是 ros service 都可以，外部調(diào)用請(qǐng)求，cartographer_ros 計(jì)算返回結(jié)果，然后外部再重啟純定位模式。
其實(shí)這個(gè)方法還是有缺陷的，它只能在機(jī)器人停在原地不動(dòng)的時(shí)候才能用，不然等匹配完，機(jī)器人已經(jīng)跑別的地方去了。
