算法思想

在Local SLAM中，通过在子图中的Scan-to-map匹配过程可以得到了一个比较理想的机器人位姿估计。 但由于Local SLAM只使用了一段时间内的局部信息，所以定位误差还会随着时间累积。为了能够进一步的降低局部误差累积的影响，Cartographer还需要通过Pixel-accurate扫描匹配来进行闭环检测， 进一步的优化机器人的位姿估计。 分支定界时，地图一定要完整

记$H=\left\{h_1...h_k...h_K \right\}$ 为雷达扫描到的 $K$个hit点集合， $h_k$是第k个hit点在scan frame下的坐标，$h_k$在地图坐标系下的坐标表示为

其中 $\varepsilon = [\varepsilon_x, \varepsilon_y,\varepsilon_{\theta}]^T$，用$T_{\varepsilon}$表示位姿估计描述的坐标变换。

Pixel-accurate扫描匹配问题可以用下式来描述:

$$\varepsilon^* = arg\ \mathop{max} \limits_{\varepsilon \in \mathcal{W}} \sum\limits_{k=1}^{K}M_{nearest}\ \ (T_{\varepsilon} h_k)$$

其中$\mathcal{W}$ 是一个搜索窗口，$M_{nearest}\$ 是离$T_{\varepsilon} h_k$最近的栅格单元的占用概率。也就是在搜索窗口$\mathcal{W}$中找到一个最优的位姿，使得hit点集合出现的概率最大化。


这里和前端的实时相关性匹配有部分相似，但是多分辨率地图和分支定界是新加入的。机器人的位姿是用$(x,y,\theta)$表示，对于暴力匹配法来说，该方法的搜索步长为1，搜索窗口的栅格索引集合 $\overline{\mathcal{W}}$ 可以通过笛卡尔积枚举：$\left\{ -w_x...w_x \right\} \times \left\{ -w_y...w_y \right\} \times \left\{ -w_{\theta}...w_{\theta} \right\}$

其中$w_x$ 和 $w_y$是x 和 y方向上的最大搜索索引，$w_{\theta}$是搜索角度的最大索引。搜索窗口$\mathcal{W}$就可以用集合 $\left\{ \varepsilon_0 + (r_{j_x}, r_{j_y}, \delta_{j_\theta}) \ | \ (j_x,j_y,j_{\theta}) \in \overline{\mathcal{W}} \right\}$ 表示。 $\varepsilon_0$是搜索中心，也是机器人位姿的初始估计， $r$ 和 $\delta_\theta$分别是位移和角度的搜索步长。

如果直接用暴力搜索就太复杂了，所以使用深度优先的分支定界法。使用一个满四叉树的概念，这跟二叉树是类似的。

其根节点代表整个搜索窗口 $\mathcal{W}$ 。树中的每一个节点的孩子都是对该节点所代表的搜索空间的一个划分， 每个叶子节点都对应着一个解。

树共有8层，分别表示从顶层（$depth=0, C_h=7$）到底层/叶子（$depth=7, C_h=0$），树的深度depth每增加1，搜索步长step就减半，表示查找的分辨率越高。每次对x和y都减半操作，像分田一样。如果栅格地图分辨率是0.05m（一个栅格的物理长度），step与depth的关系如下：

$$step = 2^{7-depth}\ *\ 0.5$$

比如在树的最底层，$C_h=0$， 此时 $c_x\le J_x<c_x+1$ 且 $c_y\le J_y<c_y+1$，显然只有1个解。

对于一个candidate，可构建四个子Candidate。C层搜索空间：

搜索树上的每个节点的上界可以通过下式计算得到：

Cartographer仍然采取了一种空间换时间的方法来进一步的提高搜索效率。预算图（precomputed grids）又叫膨胀图，从上面我们知道了树的每一层都对应一个步长，根据这个步长，生成一个对应的膨胀图。步长越大，膨胀级别越高。 根节点的分辨率是最粗的。Cartographer还为每个子图预先计算了不同尺度下的占用概率，以后的搜索过程只需要简单的查表就可以完成了。

需要注意的是膨胀不会改变栅格图的分辨率，改变的仅仅是栅格图中像素值而已。这个膨胀的过程比较复杂，源码里是用一种滑动窗口的机制实现的。

整个搜索过程借助了一个栈{C}来完成，首先将整个搜索空间$\mathcal{W}$分割，得到初始的子空间节点集合 $C_0$。 然后对集合 $C_0$ 中的节点根据它们的上界进行排序，并依次入栈{C}保证得分最高的节点在栈顶的位置。这样可以优先搜索最有可能的解空间。

最底层的节点是所有候选的评分， 这是以两个叶子节点做示范。 父节点的贪心得分永远大于子节点的贪心得分。从而保证，一旦父节点的贪心得分小于best_score，那么这个父节点的子树全部被剪枝，因为其子树的叶子节点的得分肯定也小于best_score。

源码

接上一篇的分支定界部分

const Candidate2D best_candidate = BranchAndBound(
    discrete_scans,   search_parameters, 
    lowest_resolution_candidates,
    precomputation_grid_stack_->max_depth(), 
    min_score);

// 初始传入的candidates为最上层的候选解
// 初始 candidate_depth = 7
// min_score 在lua里设置
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
    const std::vector<DiscreteScan2D>&  discrete_scans,
    const SearchParameters&  search_parameters,
    const std::vector<Candidate2D>&  candidates,
    const int  candidate_depth,
    float  min_score) const
{
  // 检查是否是最底层(叶子层)，如果已经查找到最底层了，则返回分数最高的候选解
  if (candidate_depth == 0)
      return *candidates.begin();
  // Candidate2D的构造
  // 参数分别为：0：scan_index，即旋转序号
  // 0：x方向的偏移序号  0：y方向的偏移数   search_parameters：搜索参数
  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
  
  best_high_resolution_candidate.score = min_score;
  // 对传入的候选人candidate进行循环，即从最上层开始遍历
  for (const Candidate2D& candidate : candidates)
  {
    // 将叶子节点与某棵分枝树的非叶子节点进行比较，如果非叶子节点的分数小于
    // 之前选出的最高分叶子节点，则直接将此非叶子节点candidate及其子树全部砍掉
    if(candidate.score <= min_score)
        break;

    // 一个容器，盛放这个节点引出的四个下一层的候选者
    std::vector<Candidate2D> higher_resolution_candidates;
    
    // 区域边长右移，相当于步长减半，进行分枝
    const int half_width = 1 << (candidate_depth - 1);
    
    // 对x、y偏移进行遍历，求出这一个candidate的四个子节点候选人（即最上面遍历的那个元素）
    for (int x_offset : {0, half_width}  )
    {  // 只能取0和half_width
      if (candidate.x_index_offset + x_offset >
          search_parameters.linear_bounds[candidate.scan_index].max_x)
           // 超出边界则break
           break;
      for (int y_offset : {0, half_width}  )
      {  // 只能取0和half_width   xy一共遍历四个子节点
        if (candidate.y_index_offset + y_offset >
            search_parameters.linear_bounds[candidate.scan_index].max_y)
            // 超出边界则break
            break;

        // 候选者依次推进来，一共4个
        // 可以看出，分枝定界方法的分枝是向右下角的四个子节点进行分枝
        higher_resolution_candidates.emplace_back(
            candidate.scan_index, candidate.x_index_offset + x_offset,
            candidate.y_index_offset + y_offset, search_parameters);
      }
    }
    // 对candidate四个子节点进行打分，并将
    // higher_resolution_candidates 按照score从大到小的顺序进行排列
    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
                    discrete_scans, search_parameters,
                    &higher_resolution_candidates);
    // 开始递归
    best_high_resolution_candidate = std::max(
        best_high_resolution_candidate,
        BranchAndBound(discrete_scans, search_parameters,
                       higher_resolution_candidates, candidate_depth - 1,
                       best_high_resolution_candidate.score));
  }
  return best_high_resolution_candidate;
}

第一步应先求取顶层的解及其对应评分（即可能位置和对应匹配置信度）。每层的当前节点的对应的评分均大于等于其所有下层枝叶节点，即上边界。由于不同分辨率地图存储格式，显然满足上边界条件。低分辨地图下的匹配置信度显然高于下层的高分辨地图下的匹配，然后采用迭代方法裁剪枝叶，直到遍历所有叶子节点。

在地图预处理时，其分辨率按照2的层数次方进行压缩的，由于地图有x和y两个方向，因此此层的一个节点，在下层会分为4个节点，即分辨率会放大2倍

参考：分枝定界图解