// 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.
// 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.1 karto grapher主要依据pose 和 distance信息创建localMap，scanMatcher（real-time correlative scan matcher）确定。
1) 依据当前的Vertex, 从Graph中找到与之相邻的所有vertex(一定距离范围内).
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的方式， 加快了闭环的查找。
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.