本文概述了Cartographer的理論及簡要的梳理了其開源實現(xiàn)代碼的邏輯袜腥，目的在于起到拋磚引玉的作用，為選擇性的研究相關(guān)理論及實現(xiàn)提供參考缓溅。

1.?Cartographer理論概述

Cartographer主要理論是通過閉環(huán)檢測來消除構(gòu)圖過程中產(chǎn)生的累積誤差[1]惠昔。用于閉環(huán)檢測的基本單元是submap。一個submap是由一定數(shù)量的laser scan構(gòu)成豹绪。將一個laser scan插入其對應(yīng)的submap時价淌，會基于submap已有的laser scan及其它傳感器數(shù)據(jù)估計其在該submap中的最佳位置。submap的創(chuàng)建在短時間內(nèi)的誤差累積被認(rèn)為是足夠小的瞒津。然而隨著時間推移蝉衣，越來越多的submap被創(chuàng)建后，submap間的誤差累積則會越來越大巷蚪。因此需要通過閉環(huán)檢測適當(dāng)?shù)膬?yōu)化這些submap的位姿進(jìn)而消除這些累積誤差病毡，這就將問題轉(zhuǎn)化成一個位姿優(yōu)化問題。當(dāng)一個submap的構(gòu)建完成時钓辆，也就是不會再有新的laser scan插入到該submap時剪验，該submap就會加入到閉環(huán)檢測中。閉環(huán)檢測會考慮所有的已完成創(chuàng)建的submap前联。當(dāng)一個新的laser scan加入到地圖中時功戚，如果該laser scan的估計位姿與地圖中某個submap的某個laser scan的位姿比較接近的話，那么通過某種?scan match策略就會找到該閉環(huán)似嗤。Cartographer中的scan match策略通過在新加入地圖的laser scan的估計位姿附近取一個窗口啸臀，進(jìn)而在該窗口內(nèi)尋找該laser scan的一個可能的匹配，如果找到了一個足夠好的匹配烁落，則會將該匹配的閉環(huán)約束加入到位姿優(yōu)化問題中乘粒。Cartographer的重點內(nèi)容就是融合多傳感器數(shù)據(jù)的局部submap創(chuàng)建以及用于閉環(huán)檢測的scan match策略的實現(xiàn)。

2.?開源代碼邏輯

Google開源的代碼包含兩個部分：cartographer[2]和cartographer_ros[3]伤塌。cartographer主要負(fù)責(zé)處理來自雷達(dá)灯萍、IMU和里程計的數(shù)據(jù)并基于這些數(shù)據(jù)進(jìn)行地圖的構(gòu)建，是cartographer理論的底層實現(xiàn)每聪。cartographer_ros則基于ros的通信機(jī)制獲取傳感器的數(shù)據(jù)并將它們轉(zhuǎn)換成cartographer中定義的格式傳遞給cartographer處理旦棉，與此同時也將cartographer的處理結(jié)果發(fā)布用于顯示或保存，是基于cartographer的上層應(yīng)用药薯。

3.?cartographer代碼結(jié)構(gòu)

common：定義了基本數(shù)據(jù)結(jié)構(gòu)以及一些工具的使用接口绑洛。

sensor：定義了雷達(dá)數(shù)據(jù)及點云等相關(guān)的數(shù)據(jù)結(jié)構(gòu)。

transform：定義了位姿的數(shù)據(jù)結(jié)構(gòu)及其相關(guān)的轉(zhuǎn)換童本。

kalman_filter:?主要通過kalman濾波器完成對IMU真屯、里程計及基于雷達(dá)數(shù)據(jù)的估計位姿的融合，進(jìn)而估計新進(jìn)的laser scan的位姿穷娱。

mapping：定義了上層應(yīng)用的調(diào)用接口以及局部submap構(gòu)建和基于閉環(huán)檢測的位姿優(yōu)化等的接口绑蔫。

mapping_2d和mapping_3d：對mapping接口的不同實現(xiàn)运沦。

4.?mapping_2d代碼邏輯

4.1 cartographer::mapping_2d:: GlobalTrajectoryBuilder

cartographer::mapping_2d::GlobalTrajectoryBuilder類主要實現(xiàn)了接收處理上層應(yīng)用傳遞的傳感器數(shù)據(jù)的主要接口：

（1）??????AddImuData用于接收處理上層應(yīng)用傳遞的IMU數(shù)據(jù)。

（2）??????AddOdometerPose用于接收處理上層應(yīng)用傳遞的里程計數(shù)據(jù)晾匠。

（3）??????AddHorizontalLaserFan用于接收處理上層應(yīng)用傳遞的雷達(dá)數(shù)據(jù)茶袒。

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

（1）??????artographer::mapping_2d::LocalTrajectoryBuilder類的對象local_trajectory_builder_用于完成局部submap的構(gòu)建。

（2）??????cartographer::mapping_2d::SparsePoseGraph類的對象sparse_pose_graph_用于完成閉環(huán)檢測及全局位姿優(yōu)化凉馆。

在AddImuData和AddOdometerPose函數(shù)的實現(xiàn)中會將接收的相應(yīng)傳感器數(shù)據(jù)傳遞給local_trajectory_builder_對象處理。在AddHorizontalLaserFan函數(shù)的實現(xiàn)中則將新進(jìn)的laser fan傳遞給local_trajector_builder_對象用于局部submap構(gòu)建亡资，如果該laser fan被成功插入到某個submap澜共，那么該laser fan被插入后的相關(guān)信息則被傳遞給sparse_pose_graph_對象用于基于閉環(huán)檢測的全局位姿優(yōu)化。

4.2 cartographer::mapping_2d::LocalTrajectoryBuilder

cartographer::mapping_2d::LocalTrajectoryBuilder類主要完成局部submap的構(gòu)建锥腻。其提供了接收處理傳感器數(shù)據(jù)的public函數(shù)：

（1）??????AddImuData用于處理IMU數(shù)據(jù)嗦董。

（2）??????AddOdometerPose用于處理里程計數(shù)據(jù)。

（3）??????AddHorizontalLaserFan用于處理雷達(dá)數(shù)據(jù)瘦黑。

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

（1）??????ScanMatch成員函數(shù)基于submap已有的laser fan估計當(dāng)前l(fā)aser fan在submap中的位置京革。

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

pose_tracker_

用于融合基于雷達(dá)數(shù)據(jù)的laser fan的局部估計位姿、IMU數(shù)據(jù)以及里程計數(shù)據(jù)幸斥，進(jìn)而估計出較優(yōu)的laser fan的位姿匹摇。

在AddImuData和AddOdometerPose函數(shù)中會將IMU數(shù)據(jù)和里程計數(shù)據(jù)傳遞給pose_tracker_進(jìn)行處理。pose_tracker通過UKF不斷融合IMU和里程計數(shù)據(jù)進(jìn)而更新當(dāng)前位姿甲葬，因此通過pose_tracker可以獲取當(dāng)前l(fā)aser fan的估計位姿的一個較好的初始化值廊勃。進(jìn)一步的，在AddHorizontalLaserFan函數(shù)中會調(diào)用ScanMatch经窖，ScanMatch函數(shù)中通過在submap中局部匹配得到的當(dāng)前l(fā)aser fan的估計位姿被pose_tracker_用來調(diào)整該laser fan的初始化值坡垫。這樣pose_tracker_通過融合多傳感器數(shù)據(jù)，進(jìn)而能夠估計出較優(yōu)的laser fan的位姿画侣。

4.3?cartographer::mapping_2d::SparsePoseGraph

cartographer::mapping_2d::SparsePoseGraph類主要完成基于閉環(huán)檢測的全局位姿優(yōu)化冰悠。其提供了接收處理新進(jìn)被插入到submap的laser fan相關(guān)信息的public函數(shù)：

（1）??????AddScan?對新進(jìn)的laser fan進(jìn)行閉環(huán)檢測及在適當(dāng)?shù)臅r候進(jìn)行全局優(yōu)化。

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

（1）??????ComputeConstraintsForScan對新近laser fan信息進(jìn)行處理并啟動閉環(huán)檢測scan match以及計算其約束配乱，進(jìn)而將約束添加到位姿優(yōu)化目標(biāo)中溉卓。

（2）??????AddWorkItem將laser fan與ComputeConstraintsForScan綁定，并將任務(wù)加入到隊列中宪卿。

（3）??????HandleScanQueue依此調(diào)度隊列中的任務(wù)的诵。

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

（5）??????RunOptimization優(yōu)化目標(biāo)佑钾。

在AddScan函數(shù)中會將laser fan相關(guān)信息與ComputeConstraintsForScan函數(shù)綁定西疤，并將綁定好的任務(wù)通過AddWorkItem函數(shù)加入到隊列中。HandleScanQueue函數(shù)則依次調(diào)度隊列中的任務(wù)休溶。第一次調(diào)用AddWorkItem時會直接啟動ComputeConstraintsForScan任務(wù)代赁，且在第一次ComputeConstraintsForScan任務(wù)時啟動HandleScanQueue調(diào)度扰她。在ComputeConstrainsScan中，通過constraint_builder_對象完成閉環(huán)檢測的scan match以及約束計算芭碍。當(dāng)所有約束計算完成時徒役，則會進(jìn)行RunOptimization優(yōu)化目標(biāo)。

4.4?Scan

Match

LocalTrajectoryBuilder中的scan match策略與SparsePoseGraph中的scan match策略是不同的窖壕。前者使用scan_matching::RealTimeCorrelativeScanMatcher忧勿，后者則使用scan_matching::FastCorrelativeScanMatcher。二者的目標(biāo)優(yōu)化均是由scan_matching::CeresScanMatcher完成瞻讽。

5.?總結(jié)

要將Cartographer的原理及實現(xiàn)詳細(xì)地講解清楚并不是短短兩千字能完成的鸳吸。為此，本文概述了Cartographer的理論及簡要的梳理了cartographer源碼的邏輯速勇，目的在于起到拋磚引玉的作用晌砾，進(jìn)而有利于選擇性的研究相關(guān)理論及實現(xiàn)。Cartographer的重點內(nèi)容是融合多傳感器數(shù)據(jù)的局部submap創(chuàng)建以及用于閉環(huán)檢測的scan match策略烦磁。重點內(nèi)容對應(yīng)的實現(xiàn)是：1）基于UKF的多傳感器數(shù)據(jù)融合對應(yīng)cartographer/kalman_filter目錄下的文件养匈；2）scan match策略對應(yīng)cartographer/mapping_2d/scan_matching目錄下的文件。后續(xù)有機(jī)會也會對這些重點內(nèi)容及實現(xiàn)進(jìn)行詳細(xì)地梳理都伪。

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