cartographer 代码思想解读（2）- 分支定界快速相关匹配

上节描述cartographer中算法中的相关匹配算法，为前端的scan-match，由于其初始位置有一定确定和分布性，故采用基本的暴力扫描方法。本节描述相关匹配一种快速实现，主要应用于loop-scan。回环检测为后端处理重要步骤，即检测当前位置是否曾经来过，即采用当前scan在历史中搜索是否匹配。故其搜索范围及其位置不确定性较大，故cartographer采用了分枝定界方法进行快速相关匹配。

分支界定基本原理

分支界定是计算机中一种快速求解的方法，其基本方法为将多个约束条件，拆分成多层。而顶层约束条件较少，下层逐渐增加条件，最后一层即支节点为最终结果，我们称之为分支。由于顶层的条件较少，故可得出N多个解，需要对每个解进行评分，并将最大的评分记录此节点，我们称之为上边界，即每层存储结果对应的评分均大于等于下层所有节点评分。我们可以初始一个下边界，即当前最好评分。每个真正评分应为最后一层支节点的评分，如果大于当前的最好评分，则更新，即更新下边界。

分支和边界基本说明，理论上均需要遍历到最后一个支节点才能求出真正解，如此与暴力匹配一致，并没有提高其性能。但由于有了边界后，当某一个节点评分小于当前最好评分，则其节点即以下所有之节点均无需考虑，即为剪枝，如此减少扫描范围。cartographer中在每一层中所有节点的评分也按照从大小排列，如果某节点评分小于当前最后评分（即下边界），则此节点以及此层以后所有节点均可裁剪。

盗一张图。

文字描述较为抽象，可参考他人多个的文档解释。

https://www.jianshu.com/p/9a8089cf7d3f
branch and bound（分支定界）算法
分枝定界图解

源码解读

代码目录：cartographer/mapping/internal/2d/scanmatching/fast_correlative_scan_matcher_2d

顶层函数

顶层函数包含两个

// 此匹配为全范围暴力匹配，无初始位置
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
    const sensor::PointCloud& point_cloud, float min_score, float* score,
    transform::Rigid2d* pose_estimate)
    
/*
input:
当前帧估计位置（里程计等提供的初始位置）
当前帧点云（即以激光雷达为坐标系的点云）
最小置信度
（grid在构造函数已经传递）

output：
置信度清单
匹配后输出位置
 */
bool FastCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const float min_score, float* score,
    transform::Rigid2d* pose_esti