cartographer探秘第四章之代码解析（五） --- 后端优化 --- 闭环约束2 --- FastCorrelativeScanMatcher2D

本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的 全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156

 

闭环约束和普通约束的计算的具体实现，普通约束的搜索空间为线性方向7米，角度为30度，闭环约束的搜索空间为10的6次方，角度为180度。

 

1 FastCorrelativeScanMatcher2D类

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h

// This is an implementation of the algorithm described in "Real-Time Correlative Scan Matching" by Olson.
// 这是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.
//它类似于RealTimeCorrelativeScanMatcher，但有一个不同的权衡：扫描匹配速度更快，因为在给定地图的预计算中投入了更多精力。 但是，此地图在构造后是不可变的。

1.1 Match() 与 MatchFullSubmap()

Match() 或 MatchFullSubmap()  -->  MatchWithSearchParameters()  -->  ComputeLowestResolutionCandidates()  --> BranchAndBound()  

MatchFullSubmap()中给定了在搜索空间的大小，xy方向10的6次方×分辨率，角度的搜索空间为180度。

bool FastCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const float min_score, float* score,
    transform::Rigid2d* pose_estimate) const {
  const SearchParameters search_parameters(options_.linear_search_window(),
                                           options_.angular_search_window(),
                                           point_cloud, limits_.resolution());
  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
                                   point_cloud, min_score, score,
                                   pose_estimate);
}

bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
    const sensor::PointCloud& point_cloud, float min_score, float* score,
    transform::Rigid2d* pose_estimate) const {
  // Compute a search window around the center of the submap that includes it
  // fully.
  // 围绕包含它的子图的中心计算搜索整个窗口
  const SearchParameters search_parameters(
      1e6 * limits_.resolution(),  // Linear search window, 1e6 cells/direction.
      M_PI,  // Angular search window, 180 degrees in both directions.
      point_cloud, limits_.resolution());
  // 计算搜索窗口的中点 把这个中点作为搜索的起点
  const transform::Rigid2d center = transform::Rigid2d::Translation(
      limits_.max() - 0.5 * limits_.resolution() *
                          Eigen::Vector2d(limits_.cell_limits().num_y_cells,
                                          limits_.cell_limits().num_x_cells));
  return MatchWithSearchParameters(search_parameters, center, point_cloud,
                                   min_score, score, pose_estimate);
}

1.2 SearchParameters

struct SearchParameters {...} 在correlative_scan_matcher_2d.h中声明：目的是　设置搜索窗口的大小。

SearchParameters::SearchParameters(const double linear_search_window,
                                   const double angular_search_window,
                                   const sensor::PointCloud& point_cloud,
                                   const double resolution)
    : resolution(resolution) {
  // We set this value to something on the order of resolution to make sure that
  // the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;

  // 求得 point_cloud 中雷达数据的 最大的值（最远点的距离）
  for (const Eigen::Vector3f& point : point_cloud) {
    const float range = point.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }

  // 求得角度搜索步长 angular_perturbation_step_size
  const double kSafetyMargin = 1. - 1e-3;
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));

  // 求得角度搜索空间的个数 num_scans，将 num_angular_perturbations 扩大了2倍
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
  num_scans = 2 * num_angular_perturbations + 1;

  // XY方向的搜索步长 num_linear_perturbations
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);

  // linear_bounds 的作用是 确定每一个 scan 的最大最小边界
  linear_bounds.reserve(num_scans);
  for (int i = 0; i != num_scans; ++i) {
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations});
  }
}

2 MatchWithSearchParameters()

MatchWithSearchParameters() 调用了correlative_scan_matcher_2d.h 的　GenerateRotatedScans()、DiscretizeScans()方法。

// The actual implementation of the scan matcher, called by Match() and 
// MatchFullSubmap() with appropriate 'initial_pose_estimate' and 'search_parameters'.
// 扫描匹配器的实际实现，由Match（）和MatchFullSubmap（）调用，
// 并带有适当的“ initial_pose_estimate”和“ search_parameters”。
// 根据搜索窗口和初始位置进行scan-match来进行位姿的优化。
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_NOTNULL(score);
  CHECK_NOTNULL(pose_estimate);

  // 将激光点旋转按照 initial_pose_estimate 的角度旋转一下
  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())));
  
  // 生成一系列的rotated scans，各种不同的角度的scan，60度或者360度范围内的旋转
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

  // 把上面的rotated scans转换到世界坐标系中,这里进行转换的时候只需要进行平移就可以了
  // 这里的离散激光点是在最细的分辨率的地图上面   
  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());

  // 计算最低分辨率中的所有的候选解 最低分辨率是通过搜索树的层数、地图的分辨率计算出来的。
  // 对于地图坐标系来说 最低分辨率=1<<h h表示搜索树的总的层数
  // 这里不但对最低分辨率的所有候选解的得分进行了计算　同时还按照从大到小排列
  const std::vector<Candidate2D> lowest_resolution_candidates =
      ComputeLowestResolutionCandidates(discrete_scans, search_parameters);

  // 调用函数BranchAndBound完成分支定界搜索，搜索的结果将被保存在best_candidate中。
  const Candidate2D best_candidate = BranchAndBound(
      discrete_scans, search_parameters, lowest_resolution_candidates,
      precomputation_grid_stack_->max_depth(), min_score);

  // 检查最优解的值，如果大于指定阈值min_score就认为匹配成功， 修改输入参数指针score和pose_estimate所指的对象。
  // 否则认为不匹配，不存在闭环，直接返回。
  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;
}

 2.1 GenerateRotatedScans()

std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud& point_cloud,
    const SearchParameters& search_parameters) {
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans);

  // 在 MatchFullSubmap 情况下，大约等于 -3.14186
  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;

  // 遍历360度
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
           delta_theta += search_parameters.angular_perturbation_step_size) {
    // 将 point_cloud 绕Z轴旋转 delta_theta 
    // 在 MatchFullSubmap 情况下，生成360度的 point_cloud
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))));
  }
  return rotated_scans;
}

2.2  DiscretizeScans()

// typedef std::vector<Eigen::Array2i> DiscreteScan2D;

std::vector<DiscreteScan2D> DiscretizeScans(
    const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
    const Eigen::Translation2f& initial_translation) {

  // discrete_scans的size 为 旋转的点云的个数
  std::vector<DiscreteScan2D> discrete_scans;
  discrete_scans.reserve(scans.size());

  for (const sensor::PointCloud& scan : scans) {
    // discrete_scans中的每一个 DiscreteScan2D 的size 设置为 当前这一帧scan的个数
    discrete_scans.emplace_back();
    discrete_scans.back().reserve(scan.size());

    for (const Eigen::Vector3f& point : scan) {
      // 对scan中的每个点进行坐标变换
      const Eigen::Vector2f translated_point =
          Eigen::Affine2f(initial_translation) * point.head<2>();
      // 将旋转后的对应的栅格的索引放入discrete_scans
      discrete_scans.back().push_back(
          map_limits.GetCellIndex(translated_point));
    }
  }
  return discrete_scans;
}

2.3 ShrinkToFit()

void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                                   const CellLimits& cell_limits) {
  CHECK_EQ(scans.size(), num_scans);
  CHECK_EQ(linear_bounds.size(), num_scans);

  // 遍历生成的旋转后的很多scan
  for (int i = 0; i != num_scans; ++i) {
    Eigen::Array2i min_bound = Eigen::Array2i::Zero();
    Eigen::Array2i max_bound = Eigen::Array2i::Zero();

    // 对每一帧scan进行遍历，确定每一帧的最大最小的坐标索引
    for (const Eigen::Array2i& xy_index : scans[i]) {
      min_bound = min_bound.min(-xy_index);
      max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
                                               cell_limits.num_y_cells - 1) -
                                xy_index);
    }

    // 每一帧scan的最大最小的坐标索引
    linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x());
    linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x());
    linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y());
    linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y());
  }
}

2.4 ComputeLowestResolutionCandidates()

得到初始子空间节点集合{C0}。 该函数在最低分辨率的栅格地图上查表得到各个搜索节点c∈{C0}的上界，并降序排列.

std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters) const {

　// 调用GenerateLowestResolutionCandidates() 生成最低分辨率层的所有可行解
  std::vector<Candidate2D> lowest_resolution_candidates =
      GenerateLowestResolutionCandidates(search_parameters);

  // 计算每个Candidates的得分 根据传入的地图在这个地图上进行搜索来计算得分
  // 按照匹配得分　从大到小　排序，返回排列好的candidates 
  ScoreCandidates(
      precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
      discrete_scans, search_parameters, &lowest_resolution_candidates);
  return lowest_resolution_candidates;
}

2.5 GenerateLowestResolutionCandidates()

整个树结构按照默认参数分为了７层，最上一层的搜索步长最大，地图分辨率越大，越向下，搜索步长越小，地图分辨率越小，最后一层的地图即为默认分辨率 0.05，最后一层的节点(叶子节点层）即为所有的可行解。

ComputeLowestResolutionCandidates　最低分辨率的候选解：指的是，最上面一层的可行解。层数为７情况下，最低分辨率为2的６次方，即size为64。

std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
    const SearchParameters& search_parameters) const {
  const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();  //64

  int num_candidates = 0;
  // search_parameters.num_scans 为 生成了 这个数量 的旋转后的scan
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {

    // X方向候选解的个数
    const int num_lowest_resolution_linear_x_candidates =
        (search_parameters.linear_bounds[scan_index].max_x -
         search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
        linear_step_size;

    // Y方向候选解的个数
    const int num_lowest_resolution_linear_y_candidates =
        (search_parameters.linear_bounds[scan_index].max_y -
         search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
        linear_step_size;
    
    // num_candidates 为最低分辨率这一层中 所有候选解的总个数
    num_candidates += num_lowest_resolution_linear_x_candidates *
                      num_lowest_resolution_linear_y_candidates;
  }

  // 遍历不同角度的scan的 X与Y方向的所有可行解，将 scan_index，当前可行解与initial_pose的偏差存入 candidates
  // 将所有可行解保存起来，可行解的结构为（对应角度，x偏移量，y偏移量，搜索参数）
  std::vector<Candidate2D> candidates;
  candidates.reserve(num_candidates);
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
         x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
         x_index_offset += linear_step_size) {
      for (int y_index_offset =
               search_parameters.linear_bounds[scan_index].min_y;
           y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
           y_index_offset += linear_step_size) {
        candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
                                search_parameters);
      }
    }
  }
  CHECK_EQ(candidates.size(), num_candidates);
  return candidates;
}

2.6 ScoreCandidates()

ScoreCandidates: 计算当前层的候选解在对应分辨率地图中的匹配得分，并按照得分从大到小的顺序排序。即，最低分辨率（最上一层）这层候选解，对应在size为64的地图中的匹配得分，最下一层候选解对应着size为1的地图中的匹配。

void FastCorrelativeScanMatcher2D::ScoreCandidates(
    const PrecomputationGrid2D& precomputation_grid,
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const {

  // 一个 candidate 表示经过 先旋转，后平移的 一个 scan
  for (Candidate2D& candidate : *candidates) {
    int sum = 0;

    // xy_index 为 一个 scan 的 每个点 对应在地图上的 索引
    for (const Eigen::Array2i& xy_index : discrete_scans[candidate.scan_index]) {

      // 旋转后的坐标点 加上 这个可行解的X与Y的偏置 ，即为 新的坐标点
      const Eigen::Array2i proposed_xy_index(
          xy_index.x() + candidate.x_index_offset,
          xy_index.y() + candidate.y_index_offset);

      // 新的坐标点  对应在 precomputation_grid 上栅格的值 的 和
      sum += precomputation_grid.GetValue(proposed_xy_index);
    }

    // 一个scan 的 sum 除以 这个scan中点的个数，即为 这个 scan 在这个 precomputation_grid 上的得分
    candidate.score = precomputation_grid.ToScore(
        sum / static_cast<float>(discrete_scans[candidate.scan_index].size() ) );
  }

  // 对candidates的score 进行 降序排列
  std::sort(candidates->begin(), candidates->end(), std::greater<Candidate2D>());
}

 

3 分支定界算法的实现　

3.1 BranchAndBound() 

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 {                            // 候选点最小评分

  // 这个函数是以递归调用的方式求解的。首先给出了递归终止的条件，就是如果搜索树高为0
  // 意味着我们搜索到了一个叶子节点。同时由于每次迭代过程我们都是对新扩展的候选点进行降序排列
  // 所以可以认为队首的这个叶子节点就是我们想要的最优解，直接返回即可。
  if (candidate_depth == 0) {
    // Return the best candidate.
    return *candidates.begin();
  }

  // 然后创建一个临时的候选点对象best_high_resolution_candidate，为之赋予最小的评分。
  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
  best_high_resolution_candidate.score = min_score;

  // 遍历所有的候选点，如果遇到一个候选点的评分很低，意味着以后的候选点中也没有合适的解。
  // 可以直接跳出循环退出了，说明没有构成闭环。
  for (const Candidate2D& candidate : candidates) {

    // 低于设置的阈值 或者 低于 上一层的可行解的最高分　的可行解不进行继续分枝（剪枝）
    if (candidate.score <= min_score) {
      break;
    }

    // 如果for循环能够继续运行，说明当前候选点是一个更优的选择，需要对其进行分支。
    // 一个容器，盛放这个节点candidate引出的四个下一层的候选者
    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
      // 如果 x_index_offset + x_offset 超过了界限，就跳过
      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;
        }

        // 候选者依次推进来，一共4个
        // 可以看出，分枝定界方法的分枝是向右下角的四个子节点进行分枝
        higher_resolution_candidates.emplace_back(
            candidate.scan_index, candidate.x_index_offset + x_offset,
            candidate.y_index_offset + y_offset, search_parameters);
      }
    }

    // 调用ScoreCandidates对新扩展的候选点定界并排序。
    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
                    discrete_scans, search_parameters,
                    &higher_resolution_candidates);


    // 递归调用BranchAndBound对新扩展的higher_resolution_candidates进行搜索。 
    // 从此处开始迭代，对分数最高的节点继续进行分支，直到最底层，然后再返回倒数第二层再进行迭代
    // 如果倒数第二层的最高分没有上一个的最底层（叶子层）的分数高，则跳过，否则继续向下进行评分

    // 以后通过递归调用发现了更优的解都将通过std::max函数来更新已知的最优解。
    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;
}

如下文字引用于参考文献5

// 从简单情况想，最上层节点就2个，树的深度就三层：2-1-0，每个父节点往下分两个子节点
// 先对最上层的调用BranchAndBound1(父节点2个（1和2），candidate_depth == 2,min_score=0.55)->best_high_resolution_candidate初始化构造->令其得分为min_score0.55
//        ->对其中一个父节点（如1）分枝得到1.1和1.2，并进行打分，按顺序放入容器higher_resolution_candidates中 如<1.1, 1.2>代表1.1分更高->
//        ->使用std::max1（0.55,BB）但并未比较，跳入下一层BB函数的调用，保留对节点2的循环（当反向回来的时候会继续循环2）
//                              |
//                              ^
//                              第二次调用BB2（父节点为<1.1, 1.2>，candidate_depth == 1, min_score=0.55）->best_high_resolution_candidate初始化构造->令其得分为min_score0.55
//                                      ->对选择其中一个父节点如1.1分枝得到1.1.1和1.1.2，并进行打分，按顺序放入容器higher_resolution_candidates中 如<1.1.2,1.1.1>代表1.1.2分最高
//                                      ->使用std::max2（0.55,BB）但并未比较，跳入下一层BB函数的调用，保留对节点1.2循环（当反向回来的时候会继续循环1.2）
//                                  |
//                                  ^
//                                  第三次调用BB3（父节点为<1.1.2,1.1.1>，candidate_depth == 0, min_score=0.55）-> 触发if (candidate_depth == 0)返回最佳叶子节点1.1.2                          
//                             ->跳回到上一层的BB2的std::max2（0.55,1.1.2的score）一般来说会比0.55大
//                             ->更新best_high_resolution_candidate为1.1.2->
//                             ->运行之前保留着的，对1.2的循环
//                             ->可能会触发if (candidate.score <= min_score) 即 1.2的得分小于1.1.2的得分 直接break 那么1.2及其子树全部被减掉
//                             ->如果没有被剪掉即上述条件不成立，那么将1.2分为两个子节点并排序<1.2.1，1.2.2>,使用std::max2（1.1.2的得分，BB）
//                                                                              |
//                                                                              ^
//                                                                              调用BB，触发if (candidate_depth == 0)返回这棵分枝树的最佳叶子节点1.2.1
//                             ->跳回到上一层的BB2的std::max2（1.1.2的得分,1.2.1的得分）叶子节点之间进行较量 假如还是1.1.2大
//                             // 即不管是减掉1.2及其子树还是将1.2分到叶子了，都是保持了best_high_resolution_candidate = 1.1.2
//          ->跳回到BB1的std::max1（0.55,1.1.2的score）->保持best_high_resolution_candidate = 1.1.2
//          ->运行之前的保留循环父节点2->有可能父节点2直接触发if (candidate.score <= min_score)即父节点2的得分小于1.1.2的得分
//          ->父节点2极其子树全部被砍掉
//          ->最终结束递归，输出最佳节点为：叶子节点1.1.2

3.2 分支定界理论

搜索顺序如图所示：

此图引用于参考文献[4]

 

优先对每层中分数最高的节点进行分支，直到最底层，之后再返回到倒数第二层中的第二个节点继续进行迭代。

因此，这是一种深度优先的分支定界的搜索，也就是论文中的　DFS branch and bound scan matcher for (BBS)

 

最终返回一个得分最高的节点，计算此节点与起始点的坐标变换，求得此节点的位姿估计pose_estimate。之后再通过ceres对这个粗匹配进行迭代计算，求得一个更精确的　pose_estimate，即为约束。

 

普通约束和闭环约束的区别在与搜索空间的不同，闭环约束的搜索空间太大，所以carto通过分支定界法进行了加速搜索，从而实现了秒级的实时闭环检测。

 

总结一下：

首先在最粗分辨率（64）上计算所有可行解的分（定界），之后通过降序排列，获得了最上层的搜索空间。

之后对这一层搜索空间进行 分枝，一个可行解通过加上 缩小一半的步长 生成4个新的可行解，之后计算新生成的可行解的分（定这层的上界）。

再次调用分枝定界方法，通过将 当前的最高分 当做 min_score 传入递归调用函数，如果这一层的某个可行解的评分 小于 传入的 min_score，则跳过这个可行解，不再会对这个 枝杈进行分枝（剪枝），因为每一个可行解的分都是其下边枝叶的上界，上界都小于 之前找到的最高分 min_score 了，那其下的所有枝叶的分都会小于这个值。

逐步完成先分枝，在定界，如果可行解的分低于当前最高分就剪枝，不再进行分枝，如果搜索深度为0了则返回。

 

REFERENCES

1 http://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=%E5%88%86%E6%94%AF%E5%AE%9A%E7%95%8C%E9%97%AD%E7%8E%AF%E6%A3%80%E6%B5%8B%E7%9A%84%E5%8E%9F%E7%90%86%E5%92%8C%E5%AE%9E%E7%8E%B0  --- 分支定界闭环检测的原理和实现 ，每块代码讲的很清楚

2 https://blog.csdn.net/xiaoma_bk/article/details/83040559 --- cartographer 添加约束 /分支界定法

3 https://blog.csdn.net/u013620235/article/details/72956929 --- Cartographer中的branch and bound算法的理解

4 https://blog.csdn.net/weixin_36976685/article/details/84994701 --- carto论文解析，分支定界讲的不错

5 https://www.freesion.com/article/4747857410/ 整理转载：分枝定界图解（含 Real-Time Loop Closure in 2D LIDAR SLAM论文部分解读及BB代码部分解读）