【轉】【泡泡機器人原創專欄-Cartographer】Cartographer理論及實現淺析

轉自【泡泡機器人SLAM】公衆號原創專欄：http://mp.weixin.qq.com/s/LdbFp-Zvkr02-_25ILb16g

【做者博客】http://remyspot.blog.51cto.com/

 

本文概述了Cartographer的理論及簡要的梳理了其開源實現代碼的邏輯，目的在於起到拋磚引玉的做用，爲選擇性的研究相關理論及實現提供參考。

1.Cartographer理論概述

Cartographer主要理論是經過閉環檢測來消除構圖過程當中產生的累積偏差[1]。用於閉環檢測的基本單元是submap。一個submap是由必定數量的laser scan構成。將一個laser scan插入其對應的submap時，會基於submap已有的laser scan及其它傳感器數據估計其在該submap中的最佳位置。submap的建立在短期內的偏差累積被認爲是足夠小的。然而隨着時間推移，愈來愈多的submap被建立後，submap間的偏差累積則會愈來愈大。所以須要經過閉環檢測適當的優化這些submap的位姿進而消除這些累積偏差，這就將問題轉化成一個位姿優化問題。當一個submap的構建完成時，也就是不會再有新的laser scan插入到該submap時，該submap就會加入到閉環檢測中。閉環檢測會考慮全部的已完成建立的submap。當一個新的laser scan加入到地圖中時，若是該laser scan的估計位姿與地圖中某個submap的某個laser scan的位姿比較接近的話，那麼經過某種 scan match策略就會找到該閉環。Cartographer中的scan match策略經過在新加入地圖的laser scan的估計位姿附近取一個窗口，進而在該窗口內尋找該laser scan的一個可能的匹配，若是找到了一個足夠好的匹配，則會將該匹配的閉環約束加入到位姿優化問題中。Cartographer的重點內容就是融合多傳感器數據的局部submap建立以及用於閉環檢測的scan match策略的實現。

 

2.開源代碼邏輯

Google開源的代碼包含兩個部分：cartographer[2]和cartographer_ros[3]。cartographer主要負責處理來自雷達、IMU和里程計的數據並基於這些數據進行地圖的構建，是cartographer理論的底層實現。cartographer_ros則基於ros的通訊機制獲取傳感器的數據並將它們轉換成cartographer中定義的格式傳遞給cartographer處理，與此同時也將cartographer的處理結果發佈用於顯示或保存，是基於cartographer的上層應用。

 

3.cartographer代碼結構

common：定義了基本數據結構以及一些工具的使用接口。

sensor：定義了雷達數據及點雲等相關的數據結構。

transform：定義了位姿的數據結構及其相關的轉換。

kalman_filter: 主要經過kalman濾波器完成對IMU、里程計及基於雷達數據的估計位姿的融合，進而估計新進的laser scan的位姿。

mapping：定義了上層應用的調用接口以及局部submap構建和基於閉環檢測的位姿優化等的接口。

mapping_2d和mapping_3d：對mapping接口的不一樣實現。

 

4.mapping_2d代碼邏輯

4.1 cartographer::mapping_2d:: GlobalTrajectoryBuilder

cartographer::mapping_2d::GlobalTrajectoryBuilder類主要實現了接收處理上層應用傳遞的傳感器數據的主要接口：

（1）AddImuData用於接收處理上層應用傳遞的IMU數據。

（2）AddOdometerPose用於接收處理上層應用傳遞的里程計數據。

（3）AddHorizontalLaserFan用於接收處理上層應用傳遞的雷達數據。

其中包含重要的對象成員：

（1）cartographer::mapping_2d::LocalTrajectoryBuilder類的對象local_trajectory_builder_用於完成局部submap的構建。

（2）cartographer::mapping_2d::SparsePoseGraph類的對象sparse_pose_graph_用於完成閉環檢測及全局位姿優化。

在AddImuData和AddOdometerPose函數的實現中會將接收的相應傳感器數據傳遞給local_trajectory_builder_對象處理。在AddHorizontalLaserFan函數的實現中則將新進的laser fan傳遞給local_trajector_builder_對象用於局部submap構建，若是該laser fan被成功插入到某個submap，那麼該laser fan被插入後的相關信息則被傳遞給sparse_pose_graph_對象用於基於閉環檢測的全局位姿優化。

 

4.2 cartographer::mapping_2d::LocalTrajectoryBuilder

cartographer::mapping_2d::LocalTrajectoryBuilder類主要完成局部submap的構建。其提供了接收處理傳感器數據的public函數：

（1）AddImuData用於處理IMU數據。

（2）AddOdometerPose用於處理里程計數據。

（3）AddHorizontalLaserFan用於處理雷達數據。

以及包含了一些重要的private成員：

（1）ScanMatch成員函數基於submap已有的laser fan估計當前laser fan在submap中的位置。

（2）cartographer::kalman_filter::PoseTracker類的對象

pose_tracker_用於融合基於雷達數據的laser fan的局部估計位姿、IMU數據以及里程計數據，進而估計出較優的laser fan的位姿。

在AddImuData和AddOdometerPose函數中會將IMU數據和里程計數據傳遞給pose_tracker_進行處理。pose_tracker經過UKF不斷融合IMU和里程計數據進而更新當前位姿，所以經過pose_tracker能夠獲取當前laser fan的估計位姿的一個較好的初始化值。進一步的，在AddHorizontalLaserFan函數中會調用ScanMatch，ScanMatch函數中經過在submap中局部匹配獲得的當前laser fan的估計位姿被pose_tracker_用來調整該laser fan的初始化值。這樣pose_tracker_經過融合多傳感器數據，進而可以估計出較優的laser fan的位姿。

 

4.3 cartographer::mapping_2d::SparsePoseGraph

cartographer::mapping_2d::SparsePoseGraph類主要完成基於閉環檢測的全局位姿優化。其提供了接收處理新進被插入到submap的laser fan相關信息的public函數：

（1）AddScan 對新進的laser fan進行閉環檢測及在適當的時候進行全局優化。

以及一些重要的私有成員：

（1）ComputeConstraintsForScan對新近laser fan信息進行處理並啓動閉環檢測scan match以及計算其約束，進而將約束添加到位姿優化目標中。

（2）AddWorkItem將laser fan與ComputeConstraintsForScan綁定，並將任務加入到隊列中。

（3）HandleScanQueue依此調度隊列中的任務。

（4）sparse_pose_graph::ConstraintBuilder constraint_builder_ 用於完成laser fan的scan match以及約束計算。

（5）RunOptimization優化目標。

在AddScan函數中會將laser fan相關信息與ComputeConstraintsForScan函數綁定，並將綁定好的任務經過AddWorkItem函數加入到隊列中。HandleScanQueue函數則依次調度隊列中的任務。第一次調用AddWorkItem時會直接啓動ComputeConstraintsForScan任務，且在第一次ComputeConstraintsForScan任務時啓動HandleScanQueue調度。在ComputeConstrainsScan中，經過constraint_builder_對象完成閉環檢測的scan match以及約束計算。當全部約束計算完成時，則會進行RunOptimization優化目標。

 

4.4 Scan Match

LocalTrajectoryBuilder中的scan match策略與SparsePoseGraph中的scan match策略是不一樣的。前者使用scan_matching::RealTimeCorrelativeScanMatcher，後者則使用scan_matching::FastCorrelativeScanMatcher。兩者的目標優化均是由scan_matching::CeresScanMatcher完成。

 

5.總結

要將Cartographer的原理及實現詳細地講解清楚並非短短兩千字能完成的。爲此，本文概述了Cartographer的理論及簡要的梳理了cartographer源碼的邏輯，目的在於起到拋磚引玉的做用，進而有利於選擇性的研究相關理論及實現。Cartographer的重點內容是融合多傳感器數據的局部submap建立以及用於閉環檢測的scan match策略。重點內容對應的實現是：1）基於UKF的多傳感器數據融合對應cartographer/kalman_filter目錄下的文件；2）scan match策略對應cartographer/mapping_2d/scan_matching目錄下的文件。後續有機會也會對這些重點內容及實現進行詳細地梳理。

 

Reference:

[1] Wolfgang Hess., Damon Kohler., Holger

Rapp., Daniel. Andor. Real-time loop closure in 2D lidar slam. ICRA, 2016.

[2] https://github.com/googlecartographer/cartographer

[3] https://github.com/googlecartographer/cartographer_ros