cartographer 處理imu資料流程

cartographer_ros
入口檔案：node_main.cc
入口函數main,如下圖：

初始化節點：cartographer_node；
Run函數載入設定檔LoadOptions()；根據設定檔處理所訂閱的Topic。例如IMU

注意cartographer是否接受imu資料，取決於if判斷的條件，這些條件是在“backpack_2d.lua”中所配置的。接著會交給HandleImuMessage函數（定義在sensor_bridge.cc中）處理，如下圖：

函數中會對frame_id作一個簡單check,然後交給函數trajectory_builder_->AddImuData處理（在trajectory_builder.h中直接實現了），如下圖：

而AddSensorData又是一個純虛函數，它是在繼承該父類的子類中去實現的，如下圖：

通過搜尋知道這個繼承關係：class CollatedTrajectoryBuilder : public TrajectoryBuilder 

在這個裡面實現了。

而 sensor_collator_->AddSensorData中，  sensor::Collator sensor_collator_;在map_builder.h中定義了。繼而找到在collator.cc中找到了實現，如下圖：

這裡就會把IMU資料排入佇列啦。std::unique_ptr是標準庫的智能指標，

注意到在collator.h中一個函數：

實現如下：

它在CollatedTrajectoryBuilder的建構函式中調用了，傳入的回呼函數名為HandleCollatedSensorData，處理整理後的感應器資料，到這裡就開始進入關鍵正題啦。

這個函數會把sensor data的rate擷取列印出來，同時對資料類型進行區分處理。繼續拿imu資料進行跟蹤。wrapped_trajectory_builder_->AddImuData，該類型是：

在標頭檔cartographer/mapping_2d/local_trajectory_builder.h中，知道GlobalTrajectoryBuilder類繼承了GlobalTrajectoryBuilderInterface類，如圖：

自然就能找到GlobalTrajectoryBuilder::AddImuData的具體實現了，如下圖：

這個函數就意味著地圖構建從全域最佳化轉到本地局部最佳化上來了。

第一步：

上面這個函數卡爾曼濾波根據時間預測pose，不斷更新imu資料，設定最大傾斜角閾值20度，超過則2D SLAM will fail。

第二步：圖最佳化

OptimizationProblem類是實現SPA（統計流程調適）閉環方法的。

上面這幾個就是很關鍵的函數，之後再具體分析。

網上相關文章連結：

http://mp.weixin.qq.com/s/LdbFp-Zvkr02-_25ILb16g

http://blog.csdn.net/u010566411/article/category/6467677

http://www.cnblogs.com/wenhust/tag/SLAM/（強烈推薦）

http://blog.csdn.net/zyh821351004/article/details/52421005

http://blog.csdn.net/jsgaobiao/article/details/53116042

http://blog.csdn.net/roadseek_zw/article/details/53316177
論文: 
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters 
部落格： 
http://blog.csdn.net/heyijia0327/article/details/40899819  pf原理講解 
http://blog.csdn.net/u010545732/article/details/17462941  pf代碼實現 
http://www.cnblogs.com/yhlx125/p/5634128.html  gmapping分析 
http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search  gmapping 分析 
其他 ： 
http://ishare.iask.sina.com.cn/f/24615049.html