[Cartographer] 8. 后端 BNB fast-CSM

1. 如何选择 BFS bnb or  DFS bnb ?

  由    什么时候可以确定 下界  来决定。

  肯定是优先选择 BFS ， 因为这样可以减少分支情况。

  1）如果每个分支都可以作为可行解，那么每个分支都可以计算下界并更新全局下界。则采用广度优先。 例如：MF 曼哈顿求解时，在 Bpai 空间中 求解旋转矩阵。

2）如果必须在叶子分支才可以计算下界，则必须采用深度优先。例如：cartographer中 ，多分辨率地图。中间的分支可以计算得到上界，因为每个分辨率地图中大栅格的值都是max{周围小栅格的值}。但是，这个并非是下界。

     比如： 大栅格 A 的得分是 100 ， 大栅格B 的得分是 99. 真实解 是 小栅格B1，得分98.  小栅格A1-A4的值都小于98.  如果我们鲁莽的使用 大栅格A 的得分作为下界进行剪支，那么我们就错过了正确解。

2.  cartographer中，大栅格的得分肯定大于=其下面的小栅格。这个是由多分辨率地图的构建方法决定的。可以举例子自己试试。

问题：

1. 计算旋转时，上下界如何计算？ 自定义误差计算方式，并推导相应的上界。或者弄成和MF一样的方法。

2. MF中，如果计算最小值，下界如何计算？好像没有这种问题。

BNB如何退出？

1. 上下界差距很小了。

2. 分支的步长很小了。

3. 已经遍历完所有的叶子节点了。例如cartographer中的bnb

1. 和同轨迹的子图进行匹配时，会有初始值，而且搜索框有一定范围。而且还会检查，该子图和当前Node是否小于某固定距离，否则不进行闭环检测。

2. 如果和其他轨迹的子图匹配，则初始值在地图中心，搜索框范围是整个地图。

====== 11.19 =======

多分辨率地图的生成：cartographer多分辨率地图生成

1. 多分辨率地图的cell数量没有变化，只是每个cell的概率值等于其左上角某个正方形内所有概率的最大值。

     例如：原分辨率地图100X100，现在想得到一个低分辨率50x50的地图。正常的做法是，四个格子合并，且大栅格的概率=max(四个小格子)。但是cartographer不一样，他没有合并成大格子，而是保持格子大小不变。只是改变了每个格子的概率值，令其等于这四个格子的最大值。哪四个格子呢？该格子位于右下，其他三个依次为其左上，上，左   的三个格子。

2. 多分辨地图为啥会扩增一部分？

======= 11.26 ======

1. fast_csm 检查闭环时，全局match和局部match 的 搜索范围有啥不同呢？

	搜索框的中心	搜索框的范围
全局match	待匹配子图的中心	xy 1e6; yaw pai
局部match	该帧的预估初始位姿	xy yaw 都是lua文件中 自己设置的

范围给大点也无所谓，反正会有 ShrinkToFit 函数来缩小范围。参考我的另外一篇博客

========= 12.11 ====

我们梳理下，从暴力匹配到分支定界的思路。

1. 给定一个初始姿态，并且给定一个范围，真实值就在初始姿态的某一个范围之内。具体是哪个值呢？

我们暴力枚举。假设范围是 20x20 ，单位是栅格数量。

声明一点：初始姿态也是表示在地图上的，因为要枚举，所以将这个取值范围栅格化。而且，栅格就是地图的栅格。

遍历这400个栅格，每个栅格代表一个pose，而且在这个pose下可以计算点云的得分。选取得分最大的那个pose即可。

2. 思路很简单。如果格子非常多怎么办？

我们将小格子变成大格子，先搜索大格子。假设一个大格子代表4个小格子，则原来的20x20小格子，变成了10x10大格子。

而且，每个大格子的概率值等于max{4个小格子的概率值}

这样我们先遍历100个大格子，选出最大得分的大格子。然后再对比4个小格子。计算量明显降低了。

3. 如果有很多层呢？会浪费很多资源。比如：已知小格子_1 得分是100， 那么得分小于100的大格子就没必要再划分计算了，因为他们的小格子肯定不可能得分>100。

再进一步，我们可以用bnb加速搜索过程。

以上，就是整体思路。

只需要掌握这个思路就可以了，至于cartographer中怎么构建多分辨率地图的，这些细节可以不用牢记，反正都是会忘记。

 

详细解释cartographer中的bnb流程：

 上图对应了代码中的注释部分

//分枝定界函数. 深度优先BNB
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    const std::vector<Candidate2D>& candidates, // 候选的格子
    const int candidate_depth, // candidates 所在的地图层数；叶子节点的 candidate_depth=0
    float min_score) const     // 全局下界
{
  //到了叶子节点,即可返回.
  if (candidate_depth == 0)
  {
    // Return the best candidate.
    return *candidates.begin(); // 如果到达叶子节点，返回得分最高的叶子节点
  }

  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
  best_high_resolution_candidate.score = min_score;

  //枚举当前分辨率所有的候选解.
  for (const Candidate2D& candidate : candidates)
  {
    // 小于全局下界，剪枝！！！
    // 由多分辨率地图的构建原则得知： 大格子的概率值=max{下属4个小格子的概率值}
    if (candidate.score <= min_score)// 因为是降序排列，后面的得分肯定更小
    {
      break;
    }

    //否则,进行分解.--分解成4个节点.
    std::vector<Candidate2D> higher_resolution_candidates;
    const int half_width = 1 << (candidate_depth - 1);
    for (int x_offset : {0, half_width})
    {
      if (candidate.x_index_offset + x_offset >
          search_parameters.linear_bounds[candidate.scan_index].max_x)
      {
        break;
      }
      for (int y_offset : {0, half_width})
      {
        if (candidate.y_index_offset + y_offset >
            search_parameters.linear_bounds[candidate.scan_index].max_y)
        {
          break;
        }
        higher_resolution_candidates.emplace_back(
            candidate.scan_index, candidate.x_index_offset + x_offset,
            candidate.y_index_offset + y_offset, search_parameters);
      }
    }

    //对分解的节点进行打分.并降序排列
    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
                    discrete_scans, search_parameters,
                    &higher_resolution_candidates);

    //循环调用分枝定界即可.  递归的方法
    // best_high_resolution_candidate.score 是全局的下界
    // 什么时候更新下界？到叶子节点时。
    // candidate_1 更新的下界，会在candidate_2 时起作用
    // 举例：假如有三层，每层分别有 1,4,16个节点。我们从第一层进入。即 candidates 只有第一层的一个节点
    // 下面进入第二层，得到四个 candidate 。即 higher_resolution_candidates
    // 然后又进入 BranchAndBound 函数，并依次处理第二层的四个节点。
    // for 循环中，先处理 candidate_1 .分成四个叶子节点。
    // ScoreCandidates 对四个叶子节点打分；
    // BranchAndBound  进行bnb，并返回四个叶子节点的最高得分，同时更新全局下界 best_high_resolution_candidate；
    // 至此，candidate_1 处理完成。
    // 接下来开始处理 candidate_2. 但是，candidate_1 中更新的全局下界此时还没更新min_score，所以还是得继续处理。
    // （新的全局下界会在 candidate_2 的子节点时发挥作用。由于 candidate_2的子节点是叶子节点，所以用不上了。）
    // 同样的， candidate_2 分成四个叶子节点，并 BranchAndBound 更新 全局下界；
    // 同理，candidate_3， candidate_4 也类似。
    // 至此，这三层节点处理完毕，16 个 叶子节点中的最高得分 = 全局下界。并将全局下界返回。
    // 第一层节点的同层还有另外三个节点。开始处理这三个节点。
    // candidate_2 分成四个子节点，针对这四个子节点 BranchAndBound , 此时就会使用新的全局下界。
    // 如果这四个子节点中，谁的得分小于全局上界，则直接剪枝，不再继续处理。
    // 以上就是bnb的整个流程。所以，这个bnb是深度优先。而且只会在叶子节点才会更新全局下界。
    // 每个节点的得分 = max{ 下属的4个子节点得分} 这是有多分辨率地图的构建规则决定的。
    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; // 整个BNB进行完了，返回
}