fast_CSM之分枝定界加速 2020-08-20

2016年的论文《Real-Time Loop Closure in 2D LIDAR SLAM》使用了一种能够大大减少计算量，并能够不遗漏最优解的方法——分枝定界。（相对于多分辨率搜索而言的）

解决的问题：现有的BF和Compute 2D slice在现有的平台无法实时计算，所以提出了分枝定界算法，通过缩小搜索范围（如何缩小范围呢？），加速回环检测。

分枝定界是一种深度优先的树搜索算法。通过对树进行剪枝，可以缩小了搜索范围，大大减小计算量。但同时又不会遗漏最优解。

主要思想：把全部可行的解空间不断分割为越来越小的子集（称为分支），并为每个子集内的解的值计算一个上界（称为定界）。在每次分支后，对凡是界限超出已知可行解值那些子集不再做进一步分支。这样，解的许多子集（即搜索树上的许多结点）就可以不予考虑了，从而缩小了搜索范围。

分枝定界思想，就是不断缩小搜索范围的过程。

贪心打分法的精髓在于

由于一个父节点的子节点永远是在父节点的基础上向右下方移动，也就是之前所说的，扩展子节点是向右下角四个方向进行扩展。那么父节点位姿变换到栅格坐标系中的点云，经过向右下角平移就可以得到子节点的点云。那么我取所有激光点右下角区域中的最大占据概率得分之和，一定比子节点的所有激光点所在栅格的占据概率得分之和要大。

论文中的公式为：（最大值之和 大于 和的最大值）当然，看不懂这个晦涩式子没有关系，能理解分枝定界足以。

补充

实际上，cartographer会提前计算出不同分辨率下的表，一共七张表。每张表都对应了不同层的层栅格地图中栅格的log-odd得分（通过延伸区域进行得到，如第四节所说）。这样你把点云中的每一个激光点旋转过去落在某个map中的区域的时候，就直接查表得到这个区域的得分作为这个激光点的得分。
七张表存放在一个容器中：在fast_correlative_scan_matcher_2d.h的PrecomputationGridStack2D中：

每一个表也都是fast_correlative_scan_matcher_2d.h的PrecomputationGrid2D类的成员变量：

这样扩展节点的时候，打分操作就可以直接实时查表了，而不是再进行旋转然后进行实时打分。能够大大提高实时性。

carto中的树结构

一共七层，深度为0-6，最上层（最粗糙）索引为6，最下层（最精致）索引为0

cartographer源代码补充

// cartographer的亮点所在！！！！！！！！！！！！！！！！！！！！！！！！！！
// 分枝定界方法
// 初始传入的candidates为最上层的候选解
// 初始candidate_depth = 7
// 初始min_score为配置参数，为=0.55
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector& discrete_scans,
const SearchParameters& search_parameters,
const std::vector& candidates, const int candidate_depth,
float min_score) const 
{
// 检查是否是最底层(叶子层)，如果已经查找到最底层了，则返回分数最高的候选解，排序在第一位的
if (candidate_depth == 0) {
// Return the best candidate.
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;
}


// 一个容器，盛放这个节点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
  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);

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

// 从简单情况想，最上层节点就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
//                                                          

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;
}