Cartographer分枝定界算法比喻理解

Cartographer的代码非常紧凑，为了加深自己的理解和印象，尝试用比喻的方法去描述出来，欢迎大家交流指正。

BnB比喻:

一家公司，有多个职位等级（对应多分辨率地图），现在要评项目奖金。

符合以下几点前提：

1. 一共有N级，顶层领导有M个。（栅格地图栈深为N，原始输入候选点为M个）
2. 除了1级的基层员工没有下属，所有人都有4个下属。（除了最高分辨率那层，每个坐标有4个细分坐标）
3. 下属的奖金一定不超过上一级领导。（父节点在父层得分一定高于子节点在子层得分）
4. 核算奖金的工作量（点云匹配计算量）不低，所以核算次数越少越好。

现需要开会找到这么一个榜样员工做内部画饼宣传：

1. 他一定是最底层最低级别的员工（最高精度地图中的候选点）。
2. 他的奖金大于K。（初始min_score为K，也就是下界，符合的候选点分数一定要高于下界）
3. 在符合上述两个条件的候选人中他的奖金最高（最佳匹配候选点）。
   结果可能找到，也可能找不到（极限设置情况下，最高层领导原始候选点的得分都小于K）。

暴力解法：

召集所有底层员工，直接统计找奖金最多的那一个。（遍历所有最高精度地图候选点计算点云匹配得分）。

分枝定界解法：

除最底层，每层的管理者都按需叫手下4个人把各自管理范围下的符合要求的基层员工信息报上来，并把其中奖金最高的那个作为自己管理范围下的最符合要求的基层员工信息往上报，一级一级套娃:）。如果没有符合的就上报一个奖金为K的假员工信息。
由于等级的特性，领导的奖金一定比手下员工奖金高。假设某X+1级领导，手下甲乙丙丁4个X级中层，奖金由高到低也是甲乙丙丁排序。设想如果甲手下统计出来有这么个基层员工张三符合要求，且他的奖金为K1（新的下界）。现在到乙统计了，乙发现自己的个别手下的奖金都不到K1，那这个别手下（不达新的下界的候选点）都不再需要核算他们自己手下的员工的奖金了（剪枝）。所以优先找到一位符合要求的员工后，管理层就可以通过比较，节省很多次核算奖金工作量。
为了达到这个目的，搜索时加入几个优化:

1. 从最高领导层开始往下找，才可能可以筛选掉一些中层领导，节省核算奖金工作量。
2. 每层领导手下有4个下属，优先从最高奖金的下属麾下继续找，这样才容易筛选其他下属。（候选点排序）
3. 搜索时为了尽快找到第一个满足条件的底层员工，所以应该尽快从高层往基层找（DFS深度优先搜索）。
4. 若某个满足条件的底层员工被找到了，就把员工奖金信息一级一级往上汇报，方便上级的同级筛选（剪枝）。

分枝定界函数比喻：
这个函数就像一个会议，由最高领导层开始发起会议，每个人都叫手下的人开相同的会议去统计信息，但保底奖金要求，会随着上级会议进度而变化。

/*
 * @brief  顶级管理层会议，尝试统计各位手下的符合要求的员工，有就有，没有就没有。
 * @param  search_parameters, 公司项目收益核算奖金的公司统一规则。
 * @param  initial_pose_estimate, 相关的主要部门
 * @param  point_cloud, 公司项目收益
 * @param  min_score, 保底奖金要求，低于这个奖金的员工就不算达标。
 * @param  score, 找到的话该员工的奖金会更新到这。
 * @param  pose_estimate, 找到的话该员工信息会更新到这。
 * @return True为找到一个员工，False为找不到。
 */
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
    SearchParameters search_parameters,
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, float min_score, float* score,
    transform::Rigid2d* pose_estimate) const {
  CHECK(score != nullptr);
  CHECK(pose_estimate != nullptr);

  1. 根据项目收益和相关的主要部门和核算奖金规则，选定确定几个相关的大领导。（这里只是大概作比喻，与细节无关）
  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      limits_, rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());

  const std::vector<Candidate2D> lowest_resolution_candidates =
      ComputeLowestResolutionCandidates(discrete_scans, search_parameters);

  2. 根据保底奖金要求，叫大领导们开会，尝试找一个基层榜样员工。
  const Candidate2D best_candidate = BranchAndBound(
      discrete_scans, search_parameters, lowest_resolution_candidates,
      precomputation_grid_stack_->max_depth(), min_score);
  3. 看看上报上来的员工是真是假，如果比保底奖金高，那就是找到了，否则上报的就是个假员工，相当于没找到。
  if (best_candidate.score > min_score) {
    *score = best_candidate.score;
    *pose_estimate = transform::Rigid2d(
        {initial_pose_estimate.translation().x() + best_candidate.x,
         initial_pose_estimate.translation().y() + best_candidate.y},
        initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
    return true;
  }
  return false;
}

/*
 * @brief  本级管理层会议，尝试统计各位手下的符合要求的员工，有就有，没有就报个踩线的假员工。
 * @param  discrete_scans, 姑且比喻为公司的项目收益吧。
 * @param  search_parameters, 公司项目收益核算奖金的公司统一规则。
 * @param  candidates, 本次参会的同级职员。
 * @param  candidate_depth, 本级代号，0为最底层打工人，每上一级代号加一。
 * @param  min_score, 上级通知的保底奖金要求，低于这个奖金的员工就不算达标。
 * @return 上报本级统计的奖金最高的达标底层员工信息。
 */
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 {
  1. 如果参会的各位本来就是基层，那么就把各位之中的奖金最高的人的信息上报上去吧，会议结束:)。
  if (candidate_depth == 0) {
    // Return the best candidate.
    return *candidates.begin();
  }

  2. 咱们先画个线，就是上级通知的目前保底奖金要求线（输入的下界）。而且本次会议一定要上报一个员工信息，咱先记录一个虚构的底层员工，奖金就是踩线的，如果手下都没有符合的人，至少要把他报上去:)。
  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
  best_high_resolution_candidate.score = min_score;
  3. 现在各位一个一个来汇报，看能不能刷新本次会议的上报人员记录。
  for (const Candidate2D& candidate : candidates) {
    4. 自己没有达标的人，你也不用叫手下继续统计了（剪枝）
    if (candidate.score <= min_score) {
      break;
    }
    5. 自己达标的人，看看你手下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);

    6. 叫你手下的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));
  }

  7. 上报咱们这次开会记录的那个人的信息上去（不管真实还是虚构）。
  return best_high_resolution_candidate;
}