整理转载：分枝定界图解（含 Real-Time Loop Closure in 2D LIDAR SLAM论文部分解读及BB代码部分解读）

论文《Real-Time Loop Closure in 2D LIDAR SLAM》

分枝定界是一种深度优先的树形搜索方法，避免了暴力搜索带来的计算量庞大等问题，成为cartographer的重要组成部分。

其很好地被用于激光slam的回环检测中。用当前的scan和全局地图global map进行匹配，以找到此帧scan的位姿。

所以目的要明确——寻找当前帧的位姿。

1.栅格地图

首先需要明确，什么是栅格地图，或者叫占据栅格地图。

https://zhuanlan.zhihu.com/p/21738718

建议读者仔细阅读上述链接。当然，可以先接着往下看。我这里总结一下上述链接的内容。

我们将地图划分成细小的栅格，每一个栅格可以用栅格中心点（grid point）表示。每一个栅格都有两个状态：

（1）free 未被占据
（2）occupied 被占据

2.回环检测

回环检测的目的是，通过当前扫描帧scan的数据，与global map全局的地图进行匹配。

3.暴力搜索

每一个交点代表一个（x, y），对于每个交点，又有一组θ需要遍历。因此就是三层for循环：
for x
----for y
------for θ
当然，我们知道计算三角函数是更耗时的。因此为了一定程度减小计算量，可以
for θ
----for x
------for y

4.评分机制

对于每一个遍历出来的(x, y, θ)，我们给它打个分。分最高的我们认为是回环上的位姿。

那么如何进行评分呢？
在这里插入图片描述
其中W为搜索范围，K表示当前的scan有K个激光点，T表示位姿。

公式中的M nearest(), 表示的就是每个激光点的小分（0.2或是0.7）是：用与他最近的地图点(grid point)的状态量表示。也就是激光点所在栅格的状态量。

将这种打分规则称为：平凡打分法
则我们认为，(x2, y2, θ2)比(x1, y1, θ1)更有可能是正确的姿态。

这样，一个个(x, y, θ)不断遍历，直至遍历完毕，找到一个得分最高的，就是我们认为的回环解。也就是当前scan的匹配位姿，是我们需要的最优解。

我们可以看到，暴力求解是有庞大的计算量的。因为你要把搜索框（search window）中的位姿以分辨率为步长全部遍历一遍。
在这里插入图片描述

4.分枝定界

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

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

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

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

5. 如何将树结构应用于搜索最优位姿中呢？

它是怎么体现于分枝定界方法呢？

1、整个搜索框：
在这里插入图片描述
2、将搜索范围重新划分，这时候步长选的大一点，分出来粗一些

3、一个父节点是向右下角的四个子节点进行分枝的
通过这一次粗分辨率的分割，我们得到了四个子节点
在这里插入图片描述
以左上角为原点，分成四份。原点即为树的根节点，也就是位姿（x0, y0）。这里暂不考虑角度，因为每次进行分枝定界之前都要规定一个角度，这个角度下的分枝定界做完以后，再以一定步长更新换角度，重新开始分枝定界。
也就是说，每一次分枝定界的过程中，角度θ是不变的。只是针对了平移

用这四个位姿，将当前帧scan激光点投影到地图坐标系，需要根据评分规则进行打分。

4、打分

4.1、初始化了一个最佳得分best_score = 0.9

4.2、贪心打分

用这四个位姿，将当前帧scan 激光点 投影到地图坐标系，需要根据评分规则进行打分。

以激光点落入的栅格为起点，向右下角延展 $4^d$ 个栅格（边长为 $2^d$ ）(其中d为打分节点所在层数。从下往上为d = 0,1,2,…) 取延伸区域中最大分数，如下：蓝色激光点打分为1.1

例如 scan和map的打分示意图：（以下是地图，不是姿态搜索框）
在这里插入图片描述

5、剪枝
0.2 和 0.5 这两个节点的分数小于best_score，那么直接将此节点统领的区域全部删除，体现在树上，就是进行了剪枝，将这个父节点的子树全部减掉。
在这里插入图片描述
6、依次类推。直到搜寻至叶子节点，找到叶子节点中的best_score，将具有此best_score的叶子节点的位姿，认为是分枝定界最优解。

（参考blog， https://blog.csdn.net/weixin_36976685/article/details/84994701）

贪心打分法的精髓在于

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

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

补充

实际上，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;
}

···