Cartographer總結(二）

1. 本文從/scan傳遞的資訊流幫助大家找到演算法的入口，在找演算法入口的同時可以幫助理解代碼框架，在ROS環境中跑cartographer一般包含cartographer和cartographer_ros兩個庫，cartographer是演算法庫，用于實作3D和2D情況下的SLAM演算法，cartographer_ros是對cartographer進行了一次打包，使得cartographer能夠在ROS的環境下使用，cartographer代碼框架詳細總結有很多博客，本文將不再詳細描述，可以參考代碼框架總結鏈接，下圖是我對代碼中幾個重要的類做的總結，括號內是類名對應的頭檔案位置，可以一層一層找到對應類，比如node實體中包含了map_builder_bridge_和extrapolators_2個重要的類實體，map_builder_bridge_包含了map_builder_重要的類，lua檔案和類都是對應的，一般1個{}對應1個類，
   

2. 原始碼中運用了很多C++的特性，比如繼承、虛函式、lambda、將函式作為引數傳遞等，其中= delete、std::move()等都是為了使得代碼更加科學嚴謹，效率更高、更難出bug，一個比較重要的特性是std::function（函式）當成變數使用，lambda還能對函式再一次打包后可以賦給std::function變數，注意在賦值時函式不執行，當std::function變數被使用時函式才執行，

3. 在大概理清各類的關系后，通過/scan的信號流便能找到演算法的入口，接下來以2D為例介紹/scan的流向，
   1)node(cartographer_ros/node.h)-HandleLaserScanMessage–類實體名(類所在頭檔案)-類函式：初始化完成后，訂閱了/scan訊息，該函式為ROS自動呼叫的函式，不對資料做任何處理
   2)sensor_bridge(cartographer_ros/sensor_bridge.h)-HandleLaserScanMessage：將ROS格式資料轉換成carto定義的資料格式，首先，根據角度和距離將一幀資料中所有點轉換到以激光雷達中心為原點的坐標系中，以點云的形式表示，將幀的時間戳換成最后一束激光的時間，ROS一般以第一束激光作為幀的時間戳，
   3)sensor_bridge(cartographer_ros/sensor_bridge.h)-HandleLaserScan：將一幀資料分成back_pack_2d.lua-> num_subdivisions_per_laser_scan個子點云
   4)sensor_bridge(cartographer_ros/sensor_bridge.h)-HandleRangefinder:從/tf中讀取激光雷達在機體（back_pack_2d.lua->tracking_frame）坐標系（有IMU時一般取IMU坐標系，只有Lidar時就取Lidar坐標系）的位姿（外參），將所有點轉換到機體坐標系
   5)trajectory_builder_(cartographer/mapping/internal/collated_trajectory_buider.h)-AddSensorData：將資料轉換成繼承于Data的模板類dispatchable，IMU、編碼器等傳感器資料都可以用該模板類表示，至此，資料流cartographer_ros進入到cartographer，該模板類像是將不同傳感器資料流合流，用一個模板表示，
   6)trajectory_builder_(cartographer/mapping/internal/collated_trajectory_buider.h)-AddData：所有傳感器資料都會進入這個函式，不對資料作處理
   7)sensor_collator_(cartographer/sensor/internal/collator.h)-AddSensorData：得到queue_key{trajectory_id,sensor_id}, trajectory_id指明第幾條軌跡，sensor_id指明是什么型別的多少號傳感器，若一種型別的傳感器只有一個，則就是ROS訊息中的frame_id，不對資料作處理，
   8)queue(cartographer/sensor/intenal/ordered_muti_queue.h)-Add：添加資料到queues_中對應queue_key，不對資料作處理
   9)queue(cartographer/sensor/intenal/ordered_muti_queue.h)-Dispatch：在一個死回圈處理函式，直到提取完已經收到的各傳感器資料，不對資料作處理，該函式體里會呼叫queues_->callback(),callback是一個函式變數，在構造trajectory_builder_時將lambda函式賦給它，而trajectory_builder_的成員函式HandleCollatedSensorData是lambda函式的函式體部分，故程式運行時，下一步會呼叫HandleCollatedSensorData，
   10)trajectory_builder_(cartographer/mapping/internal/collated_trajectory_buider.h)-HandleCollatedSensorData：列印"rate:",不對資料作處理
   11)data(cartographer/sensor/internal/dispatchable)-AddToTrajectoryBuilder：不對資料作任何處理
   12)wrapped_trajectory_builder_(cartographer/mapping/internal/global_trajectory_builder.cc)-AddSensorData：至此資料流才算進入演算法的主體，進入官方鏈接中的演算法流程圖，如下圖
   

13)local_trajectory_builder_( mapping/internal/2d/local_trajectory_buider_2d.h)-AddRangeData：去運動畸變（首先通過extrapolator預測每一束光束得到的點在世界坐標系位置并記錄此刻Lidar中心在世界坐標系位姿，最后將所有點轉換到最后一束激光打出去時刻對應Lidar坐標系中，相當于用PoseExtrapolator得到每束激光對應坐標系與最后一束光的相對位姿，要求PoseExtrapolator比Lidar高頻且準確）、重力對齊、體素濾波(Voxel Filter)、拼接trajectory_builder_2d.lua->num_accumlated_range_data個3)中分出的子點云（我想這也是為什么要用最后一束光束作當前子點云時間戳的原因，子點云最后一束時間戳也是累積點云最后一束激光的時間戳，反之，若用子點云第一束光束時間戳，那么它并不是累積點云的第一束激光的時間戳）
14)local_trajectory_builder_( mapping/internal/2d/local_trajectory_buider_2d.h)-AddAccumulatedRangeData：自適應體素濾波(Adaptive Voxel Filter，在滿足min_num_points情況下，使體素濾波尺寸盡可能大且不超過max_length)、Scan Matching(在ceres scan matching前可以通過暴力匹配得到更好的初值)、運動濾波(Motion Fiter,時間、距離和角度中有一個超出設定值便插入關鍵幀，否則Dropped)、插入關鍵幀更新active_submap(由2個submap組成)、得到IserttionResult
15)pose_graph_(mapping/internal/2d/pose_graph_2d.h)-AddNode：插入關鍵幀、計算約束、后端優化（Global SLAM）
16)wrapped_trajectory_builder_(cartographer/mapping/internal/global_trajectory_builder.cc)-local_slam_result_callback_：該函式也是被一個Lambda函式賦值，Lambda函式的函式體呼叫了map_builder_bridge_(cartographer_ros/map_builder_bridge.h)-OnLocalSlamResult,這樣cartographer演算法庫得到的結果回傳到cartographer_ros，