相關注釋代碼鏈接為：cartographer代碼注釋

代碼主要分為兩個部分，其一為cartographer的核心實現，另一個為cartographer的ros封裝殼。首先介紹其ros封裝，可以看到大概的調用流程，然后再深入源碼去剖析其實現過程。但是其代碼可以說十分的繁瑣且復雜，在只能大致理清楚其邏輯。

Cartographer-ROS

根據運行的命令 roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag可以知道，運行文件offline_backpack_2d.launch啟動算法。

• 加載配置文件 backpack_2d.lua
• 調用launch文件 offline_node.launch

因此，深入文件 offline_node.launch

• 運行節點 rviz
• 運行節點 cartographer_occupancy_grid_node
• 運行節點 cartographer_offline_node

因此對于cartographer-ros來說，主要的節點就是這兩個，其在如下文件中分別實現

• offine_node_main.cc
• occupancy_grid_node.cc

occupancy_grid_node

主要實現的功能有

• 新建一個定時器，定時發布全局地圖信息
• 構造回調函數，在回調函數內處理子地圖列表信息

其子列表信息回調函數流程如下：

• 設置所有之前的子地圖為待刪除子地圖
• 在待刪除子地圖中去掉當前子地圖列表中還存在的
• 獲取新子地圖信息 FetchSubmapTextures
  • 發布一個srv，獲取壓縮后的子地圖柵格信息
  • 對柵格地圖信息進行解壓
• 轉換子地圖信息格式

而發布流程則為

• 將所有子地圖構造成一個圖片(調用cairo實現)
• 轉換為ROS格式并發布

offine_node_main

在main函數中

• 調用函數CreateMapBuilder構造了一個MapBuilder類
• 調用RunOfflineNode函數，傳入MapBuilder類指針

其中，RunOfflineNode函數則為離線運行節點的主要邏輯

可以從圖中看到，函數主要工作為

• 調用AddOffineTrajectory()函數，實際上該函數主要調用的是map_builder類的AddTrajectoryBUilder函數。
• 創建ROS相關的發布以及訂閱消息的處理，并定時發布可視化信息
• 構造SensorBridge類，并處理傳感器數據,實際上則是調用TrajectoryBuilderInterface類的傳感器數據處理
• 讀取參數配置文件，保存地圖信息等其他工作

從輸入輸出的角度來看整體的代碼，我們需要弄清楚本節點發布的ROS數據具體有哪些，是如何獲得的，并且是如何處理這些傳感器數據。

輸入

• 傳遞傳感器數據到核心cartographer代碼
  直接由SensorBridge類使用TrajectoryBuilderInterface類指針調用其傳感器數據處理函數。

輸出

• 發布消息到ROS
  • 定時發送軌跡、子地圖列表、約束項等信息
  • 在請求時返回子地圖的柵格地圖數據,調用函數HandleSubmapQuery，實際上調用的是map_builder_->SubmapToProto函數獲取壓縮后的柵格信息

因此，整理如下

• 調用map_builder類的AddTrajectoryBuilder函數,進行初始化
• 調用TrajectoryBuilderInterface類相關的傳感器處理函數處理傳感器信息
• 調用map_builder_->SubmapToProto函數等獲取處理結果

最終，我們可以看到實際上交互的內容并不多，在獲取柵格地圖信息上由于數據量較大進行了數據壓縮，其他的都是直接通過指針獲取得到數據。因此我們接下來看主體代碼時只要集中在前面兩點上即可。

Cartographer 主體

根據上文結論，接下來分為兩個部分進行介紹。

調用map_builder類的AddTrajectoryBuilder函數

這里根據配置參數的選擇，才有了2D激光數據和3D激光數據的區別，根據不同的傳感器數據配置2D或者3D對應的類，由于在代碼上沒有太大的區別邏輯都是幾乎一模一樣的（在處理IMU數據上有一些區別，3D激光必須要IMU,2D可以不要）。因此后面都是以2D類舉例解釋。

構造局部軌跡生成類 LocalTrajectoryBuilder2D

使用局部軌跡生成類，構造全局軌跡生成類 GlobalTrajectoryBuilder

使用全局軌跡生成類，構造軌跡生成管理類指針 CollatedTrajectoryBuilder

將軌跡生成管理類指針保存在一個vector中

這里就非常的繞，需要仔細思考，所有的軌跡生成類均為TrajectoryBuilderInterface的子類，因此需要特別注意，使用TrajectoryBuilderInterface類指針是具體指向的是哪一個子類的實現。最后返回的是CollatedTrajectoryBuilder類的指針，因此下面調用的是該之類的傳感器數據處理函數。

• 其他工作
  • 純定位模式的配置
  • 初始位置的設置

調用TrajectoryBuilderInterface類函數處理傳感器數據

由上面分析可知這里的TrajectoryBuilderInterface類是父類，而實際調用的是子類CollatedTrajectoryBuilder。

CollatedTrajectoryBuilder類

主要功能：

• 構造Collator類，管理所有的傳感器數據，設置回調函數為HandleCollatedSensorData(),在Collator類內所有的傳感器數據都被表達成通用的傳感器數據結構。
• 調用Collator類處理傳感器數據，在將傳感器數據轉換成通用數據后，調用回調函數HandleCollatedSensorData()
• 在回調函數中調用全局軌跡生成類GlobalTrajectoryBuilder對傳感器數據進行處理

GlobalTrajectoryBuilder類

在全局軌跡生成類中將傳感器數據進行了分類，其中激光雷達數據、IMU數據和里程計數據用于生成局部軌跡，其他的傳感器數據如GPS信息、路標點信息等則直接被添加到了位姿圖優化類PoseGraph2D類中。

• 針對里程計、IMU和激光信息，調用局部路徑生成LocalTrajectoryBuilder2D類進行處理
• 處理結束后將其添加到PoseGraph2D類中進行優化
• 將其他傳感器數據同樣添加到PoseGraph2D類中進行優化

因此，具體的實現部分在LocalTrajectoryBuilder2D類中添加傳感器數據部分，以及PoseGraph2D類中添加傳感器數據作為節點部分。

LocalTrajectoryBuilder2D類

• 構造PoseExtrapolator位姿外推類，類似于卡爾曼濾波器的功能，利用之前的傳感器信息和當前激光采集時間，預測當前位置和速度。
• 根據預測速度，對激光數據進行運動畸變矯正
• 調用CeresScanMatcher2D類，進行激光數據前端匹配的計算當前幀與當前子地圖的位置關系。
• 激光達到一定距離則調用submap_2d類插入到子地圖中
• 利用匹配結果更新PoseExtrapolator的估計，為下一次做準備

其中，CSM前端匹配算法是，子地圖-激光幀的匹配。具體原理部分可參考原論文，這里的重點不是理論。另外submap_2d類插入激光數據到子地圖中，對子地圖進行了管理，具體的子地圖管理策略為。

其中，激光數據插入地圖調用的是probability_grid_range_data_inserter_2d類中的函數，其原理為占用柵格地圖更新原理，可參考注釋和相關資料理解，這里不再贅述。

PoseGraph2D類

• 在收到局部軌跡生成類得到的子地圖后，首先添加到構造的優化問題OptimizationProblem2D類中。
• 然后調用fast_correlative_scan_matcher_2d.cc文件中的算法進行回環，主要就是利用分支定界算法在一定大小的窗口內進行搜索匹配。
• 回環檢測結束后，無論是否成功都將優化求解問題添加到線程池中
• 其他傳感器數據采集到也會將優化問題添加到線程池中進行求解

分支定界方法用于尋找回環約束的具體實現與原理較為復雜，可以參考論文和代碼注釋進行學習，這同樣不是本文的重點。最后，將所有傳感器的數據添加到OptimizationProblem2D類中構造了后端優化問題，接下來我們將求解這樣一個最終的優化問題。

OptimizationProblem2D類

優化問題的求解調用函數OptimizationProblem2D::Solve，其主要流程和普通的Ceres優化流程沒有什么區別，就是按照Ceres的套路來。其中最為關鍵問題在于圖優化中的節點和邊如何構造以及誤差函數的計算，這里我們采用因子圖的方式表達這樣一個圖模型。

最后我們看一下誤差函數,定義在SpaCostFunction2D類中，非常非常的簡單，就是優化結果不能和之前匹配得到的相對位置相差太多。

bool operator()(const T* const start_pose, const T* const end_pose,T* e) const {// 誤差計算函數 // ScaleError 基于旋轉和平移項不同的權重，保證收斂// ComputeUnscaledError 真正計算誤差的函數const std::array<T, 3> error =ScaleError(ComputeUnscaledError(transform::Project2D(observed_relative_pose_.zbar_ij),start_pose, end_pose),observed_relative_pose_.translation_weight,observed_relative_pose_.rotation_weight);// 保存誤差std::copy(std::begin(error), std::end(error), e);return true;}

這里里程計的誤差函數和匹配的誤差函數是相同的，可以認為結果是兩個里程計的可變加權和，其他的誤差函數如路標點等這里不再過多展開，都是一樣的。

至此，我們將cartographer代碼的整體流程過了一遍，其原理不難，但是源碼過于復雜，其中還有許許多多的細節，需要花費大量時間仔細閱讀才能真正熟悉它。

總結

以上是生活随笔為你收集整理的cartographer 代码分析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。