相關(guān)注釋代碼鏈接為：cartographer代碼注釋

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

Cartographer-ROS

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

• 加載配置文件 backpack_2d.lua
• 調(diào)用launch文件 offline_node.launch

因此，深入文件 offline_node.launch

• 運(yùn)行節(jié)點(diǎn) rviz
• 運(yùn)行節(jié)點(diǎn) cartographer_occupancy_grid_node
• 運(yùn)行節(jié)點(diǎn) cartographer_offline_node

因此對(duì)于cartographer-ros來說，主要的節(jié)點(diǎn)就是這兩個(gè)，其在如下文件中分別實(shí)現(xiàn)

• offine_node_main.cc
• occupancy_grid_node.cc

occupancy_grid_node

主要實(shí)現(xiàn)的功能有

• 新建一個(gè)定時(shí)器，定時(shí)發(fā)布全局地圖信息
• 構(gòu)造回調(diào)函數(shù)，在回調(diào)函數(shù)內(nèi)處理子地圖列表信息

其子列表信息回調(diào)函數(shù)流程如下：

• 設(shè)置所有之前的子地圖為待刪除子地圖
• 在待刪除子地圖中去掉當(dāng)前子地圖列表中還存在的
• 獲取新子地圖信息 FetchSubmapTextures
  • 發(fā)布一個(gè)srv，獲取壓縮后的子地圖柵格信息
  • 對(duì)柵格地圖信息進(jìn)行解壓
• 轉(zhuǎn)換子地圖信息格式

而發(fā)布流程則為

• 將所有子地圖構(gòu)造成一個(gè)圖片(調(diào)用cairo實(shí)現(xiàn))
• 轉(zhuǎn)換為ROS格式并發(fā)布

offine_node_main

在main函數(shù)中

• 調(diào)用函數(shù)CreateMapBuilder構(gòu)造了一個(gè)MapBuilder類
• 調(diào)用RunOfflineNode函數(shù)，傳入MapBuilder類指針

其中，RunOfflineNode函數(shù)則為離線運(yùn)行節(jié)點(diǎn)的主要邏輯

可以從圖中看到，函數(shù)主要工作為

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

從輸入輸出的角度來看整體的代碼，我們需要弄清楚本節(jié)點(diǎn)發(fā)布的ROS數(shù)據(jù)具體有哪些，是如何獲得的，并且是如何處理這些傳感器數(shù)據(jù)。

輸入

• 傳遞傳感器數(shù)據(jù)到核心cartographer代碼
  直接由SensorBridge類使用TrajectoryBuilderInterface類指針調(diào)用其傳感器數(shù)據(jù)處理函數(shù)。

輸出

• 發(fā)布消息到ROS
  • 定時(shí)發(fā)送軌跡、子地圖列表、約束項(xiàng)等信息
  • 在請(qǐng)求時(shí)返回子地圖的柵格地圖數(shù)據(jù),調(diào)用函數(shù)HandleSubmapQuery，實(shí)際上調(diào)用的是map_builder_->SubmapToProto函數(shù)獲取壓縮后的柵格信息

因此，整理如下

• 調(diào)用map_builder類的AddTrajectoryBuilder函數(shù),進(jìn)行初始化
• 調(diào)用TrajectoryBuilderInterface類相關(guān)的傳感器處理函數(shù)處理傳感器信息
• 調(diào)用map_builder_->SubmapToProto函數(shù)等獲取處理結(jié)果

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

Cartographer 主體

根據(jù)上文結(jié)論，接下來分為兩個(gè)部分進(jìn)行介紹。

調(diào)用map_builder類的AddTrajectoryBuilder函數(shù)

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

構(gòu)造局部軌跡生成類 LocalTrajectoryBuilder2D

使用局部軌跡生成類，構(gòu)造全局軌跡生成類 GlobalTrajectoryBuilder

使用全局軌跡生成類，構(gòu)造軌跡生成管理類指針 CollatedTrajectoryBuilder

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

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

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

調(diào)用TrajectoryBuilderInterface類函數(shù)處理傳感器數(shù)據(jù)

由上面分析可知這里的TrajectoryBuilderInterface類是父類，而實(shí)際調(diào)用的是子類CollatedTrajectoryBuilder。

CollatedTrajectoryBuilder類

主要功能：

• 構(gòu)造Collator類，管理所有的傳感器數(shù)據(jù)，設(shè)置回調(diào)函數(shù)為HandleCollatedSensorData(),在Collator類內(nèi)所有的傳感器數(shù)據(jù)都被表達(dá)成通用的傳感器數(shù)據(jù)結(jié)構(gòu)。
• 調(diào)用Collator類處理傳感器數(shù)據(jù)，在將傳感器數(shù)據(jù)轉(zhuǎn)換成通用數(shù)據(jù)后，調(diào)用回調(diào)函數(shù)HandleCollatedSensorData()
• 在回調(diào)函數(shù)中調(diào)用全局軌跡生成類GlobalTrajectoryBuilder對(duì)傳感器數(shù)據(jù)進(jìn)行處理

GlobalTrajectoryBuilder類

在全局軌跡生成類中將傳感器數(shù)據(jù)進(jìn)行了分類，其中激光雷達(dá)數(shù)據(jù)、IMU數(shù)據(jù)和里程計(jì)數(shù)據(jù)用于生成局部軌跡，其他的傳感器數(shù)據(jù)如GPS信息、路標(biāo)點(diǎn)信息等則直接被添加到了位姿圖優(yōu)化類PoseGraph2D類中。

• 針對(duì)里程計(jì)、IMU和激光信息，調(diào)用局部路徑生成LocalTrajectoryBuilder2D類進(jìn)行處理
• 處理結(jié)束后將其添加到PoseGraph2D類中進(jìn)行優(yōu)化
• 將其他傳感器數(shù)據(jù)同樣添加到PoseGraph2D類中進(jìn)行優(yōu)化

因此，具體的實(shí)現(xiàn)部分在LocalTrajectoryBuilder2D類中添加傳感器數(shù)據(jù)部分，以及PoseGraph2D類中添加傳感器數(shù)據(jù)作為節(jié)點(diǎn)部分。

LocalTrajectoryBuilder2D類

• 構(gòu)造PoseExtrapolator位姿外推類，類似于卡爾曼濾波器的功能，利用之前的傳感器信息和當(dāng)前激光采集時(shí)間，預(yù)測(cè)當(dāng)前位置和速度。
• 根據(jù)預(yù)測(cè)速度，對(duì)激光數(shù)據(jù)進(jìn)行運(yùn)動(dòng)畸變矯正
• 調(diào)用CeresScanMatcher2D類，進(jìn)行激光數(shù)據(jù)前端匹配的計(jì)算當(dāng)前幀與當(dāng)前子地圖的位置關(guān)系。
• 激光達(dá)到一定距離則調(diào)用submap_2d類插入到子地圖中
• 利用匹配結(jié)果更新PoseExtrapolator的估計(jì)，為下一次做準(zhǔn)備

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

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

PoseGraph2D類

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

分支定界方法用于尋找回環(huán)約束的具體實(shí)現(xiàn)與原理較為復(fù)雜，可以參考論文和代碼注釋進(jìn)行學(xué)習(xí)，這同樣不是本文的重點(diǎn)。最后，將所有傳感器的數(shù)據(jù)添加到OptimizationProblem2D類中構(gòu)造了后端優(yōu)化問題，接下來我們將求解這樣一個(gè)最終的優(yōu)化問題。

OptimizationProblem2D類

優(yōu)化問題的求解調(diào)用函數(shù)OptimizationProblem2D::Solve，其主要流程和普通的Ceres優(yōu)化流程沒有什么區(qū)別，就是按照Ceres的套路來。其中最為關(guān)鍵問題在于圖優(yōu)化中的節(jié)點(diǎn)和邊如何構(gòu)造以及誤差函數(shù)的計(jì)算，這里我們采用因子圖的方式表達(dá)這樣一個(gè)圖模型。

最后我們看一下誤差函數(shù),定義在SpaCostFunction2D類中，非常非常的簡(jiǎn)單，就是優(yōu)化結(jié)果不能和之前匹配得到的相對(duì)位置相差太多。

bool operator()(const T* const start_pose, const T* const end_pose,T* e) const {// 誤差計(jì)算函數(shù) // ScaleError 基于旋轉(zhuǎn)和平移項(xiàng)不同的權(quán)重，保證收斂// ComputeUnscaledError 真正計(jì)算誤差的函數(shù)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;}

這里里程計(jì)的誤差函數(shù)和匹配的誤差函數(shù)是相同的，可以認(rèn)為結(jié)果是兩個(gè)里程計(jì)的可變加權(quán)和，其他的誤差函數(shù)如路標(biāo)點(diǎn)等這里不再過多展開，都是一樣的。

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。