原文鏈接：http://blog.csdn.net/zyh821351004/article/details/52421005
cartographer與karto的比較
1. 兩者采取的都是圖優化框架。 采取的優化庫不一致， karto采取的是spa(karto_slam)或g2o(nav2d), cartographer采取的是google的ceres構建problem優化。 karto的前端與后端采取的是單線程進行，cartographer按paper說明，采取的是4線程后端優化，還在進一步確定。
2. 運動預測部分：tracker
karto利用的是odom進行初始位置的預測， cartographer部分利用imu構建預測模型，scanmatcher與odom(可選)構建觀測模型，采取UKF進行運動預測， cartographer帶有tracker的說法。
// Implementation of a Kalman filter. We follow the nomenclature from Thrun, S. et al., Probabilistic Robotics, 2006.
// Extended to handle non-additive noise/sensors inspired by Kraft, E., A // Quaternion-based Unscented Kalman Filter for Orientation Tracking.
3. scanMatcher 部分
3.1 karto 采取的的是real-time correlative scan matcher（三維窗口遍歷尋優）的方式進行的。 采取的是雙分辨率的低分辨率和高分辨率的兩次搜索。
This is an implementation of the algorithm described in "Real-Time Correlative Scan Matching" by Olson. The correlative scan matching algorithm is exhaustively evaluating the scan matching search space. As described by the paper, the basic steps are: // 1) Evaluate the probability p(z|xi, m) over the entire 3D search window using the low-resolution table. // 2) Find the best voxel in the low-resolution 3D space that has not already been considered. Denote this value as Li. If Li < Hbest, terminate: Hbest is the best scan matching alignment. // 3) Evaluate the search volume inside voxel i using the high resolution table. Suppose the log-likelihood of this voxel is Hi. Note that Hi <= Li since the low-resolution map overestimates the log likelihoods. If Hi > Hbest, set Hbest = Hi. This can be made even faster by transforming the scan exactly once over some discretized range.
3.2 cartoGrapher也是采取的雙搜索的方式進行的， 先用一次real-time correlative scan matcher（三維窗口遍歷尋優），再構建優化等式，利用ceres優化求解。（柵格概率， T的偏差，R的偏差）
occupied_space_cost_functor_weight TranslationDeltaCostFunctor RotationDeltaCostFunctor
4. submap的說明
4.1 karto沒有submap的概念，全部以keyScan的形式存儲在sensorManager。 無地圖緩存，但每次計算地圖有計算消耗。
采取的是scan-map的匹配方式，每次keyScan進入主動的依據pose的距離窗口生成localMap進行匹配。 local 與 gloal的loop closure依據graph的結構和sensorManage順序存儲分配的ID信息，選擇候選scans，生成localMap，進行匹配，依據score進一步確定閉環。
4.2 . cartographer采用了submap的概念， 依據一定數量的scan初始一個submap, 依據窗口大小， 插入newScan，更新submap. 有子圖緩存，會占用內存。
// An individual submap, which has an initial position 'origin', keeps track of which laser fans where inserted into it, and sets the 'finished_probability_grid' to be used for loop closing once the map no longer changes.
// Submaps is a sequence of maps to which scans are matched and into which scans are inserted. // Except during initialization when only a single submap exists, there are // always two submaps into which scans are inserted: an old submap that is used // for matching, and a new one, which will be used for matching next, that is // being initialized. // // Once a certain number of scans have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap // is now the "old" submap and is used for scan-to-map matching. Moreover, // a "new" submap gets inserted.
5. loopCheck
5.1 karto grapher主要依據pose 和 distance信息創建localMap，scanMatcher（real-time correlative scan matcher）確定。
1) 依據當前的Vertex, 從Graph中找到與之相鄰的所有vertex(一定距離范圍內). 2) 采取廣度優先搜索的方式，將相鄰（next）與相連（adjacentVertices）添加進nearLinkedScans. 3) 從sensorManager中取從前到后，依據id序號挑選與當前在一定距離范圍內，且不在nearLinkedScans中的candidateScans, 當數量達到一定size，返回。 4）loopScanMatcher進行scanTomap的匹配，當匹配response 和covariance達到一定要求認為閉環檢測到。得到調整的correct pose. 5）Add link to loop : 調整邊（全局閉環） 6) 觸發correctPose: spa優化
5.2 cartogapher 類似（（real-time correlative scan matcher）），引入了branch and bound的方式， 加快了閉環的查找。
依據多分辨率多層的樹型結構，單枝生長的方式(branch)，及時剪枝操作（bound），深度優先搜索確定閉環。 （Intra-submap Inter-submap ）
添加相應的閉環約束。構建優化問題，利用ceres優化。
// Current optimization problem.
sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
// This is an implementation of the algorithm described in "Real-Time Correlative Scan Matching" by Olson.
// It is similar to the RealTimeCorrelativeScanMatcher but has a different trade-off: Scan matching is faster because more effort is put into the
// precomputation done for a given map. However, this map is immutable after construction.
[原文：http://blog.csdn.net/zyh821351004/article/details/52421005 ]
安裝：
github: https://github.com/googlecartographer/cartographer_ros
安裝指導 https://google-cartographer-ros.readthedocs.io/en/latest/
tip: catkin_make_isolate 編譯帶非ros repo的 ＝》 可以試試 catkin build 編譯
rosbag 數據集出錯（bag broke）：校驗
shasum ~/Downloads/cartographer_paper_deutsches_museum.bag
2a021fadae9deb0643d73c8ca3acb332fcb3baa2
demo_video:
cartographer 3d http://v.qq.com/x/page/n0334yt1tt1.html
cartographer 2d http://v.qq.com/x/page/z03346p134v.html
https://github.com/googlecartographer/cartographer_ros/issues/41 sensor: two Hokuyo UTM-30LX-EW + 3DM GX4 25
地圖保存接口（srv）: 保存位置：～/.ros/cartographer_tb.pgm
rosservice call /finish_trajectory "stem: 'cartographer_tb'"
最早調試遇到的bug，issue貼的圖片。
