Cartographer中的branch and bound算法的理解

target： 寻找从scan坐标系到submap坐标系的最佳变换参数

构建Candidate树

搜索空间由Candidate构成的树来描述。

先看Candidate的数据结构
int x_index_offset; //x平移量
int y_index_offset; //y平移量
int scan_index; //记录相对应的旋转点云的index, 相当于旋转参数

树顶层构建

树顶层构建取决于SearchParameters，在MatchFullSubmap（）函数中对其初始化

const SearchParameters search_parameters(
          1e6 * limits_.resolution(),//决定了x,y方向的搜索范围，1000000个cell（就是pixel）.
          M_PI,  // Angular search window, 180 degrees in both directions.
          point_cloud, limits_.resolution());

顶层x平移量的stepsize如下：
linear_step_size = 1 << precomputation_grid_stack_->max_depth();
其中depth是自己定义的，如果是8，则linear_step_size=256;
那么顶层的线性搜索空间大小就是1000000/256，大约4000。
角度搜索步长由cell大小（1 pixel），和scan的最大扫描距离决定。
角度空间大小大概是300；
所以顶层candidate数量是：4000×4000×300

其余层的构建。

每一个candidate可构建四个子Candidate。角度参数不变，对x_index_offset，y_index_offset加入新的偏移量：0和half_width，后者即 1 << (candidate_depth - 1)。 逐层实现搜索空间的细化。

这里引入两个概念：
node： 即Candidate
node树： 这个node本身和其下面所有层的node的集合。

最底层的node全体构成了整个搜索空间，即1e6×1e6×300

构建precomputation_grid_stack_

层数与Candidate树相当，每一层的大小相同，分辨率由顶层到底层递增，最后一层就是probability_grid，即submap。
每一层的precomputation_grid_计算：对depth×depth窗口内的probability_grid计算最大值，赋值给窗口中的没一个像素，precomputation_grid_的可视化图可见论文。

计算candidate得分

由同层precomputation_grid_可计算candidate的得分。
这种计算方式所得的分数即node树中的最大得分，即upperbound。原理可见文章最后的图解。

搜索算法

实现寻找从scan坐标系到submap坐标系的最佳变换参数
1. 构建顶层Candidate，计算所有Candidate的得分，从大到小排序。
2. 对第一个Candidate构建“子Candidate”，计算这四个新Candidate的得分，从大到小排序。
3. 重复 2 ，直到stepsize=1，得到最底层的4个Candidate，取得分最大的作为best_high_resolution_candidate。
4. 返回倒数第二层，将best_high_resolution_candidate与剩下的3个Candidate逐个对比，如果best_high_resolution_candidate的score大于其中任何一个Candidate，则无需进入它们的分支，否则进入分支，有可能找到score更高的Candidate，把它设置为best_high_resolution_candidate。
5. 重复 4 ，直到遍历整个Candidate树，返回best_high_resolution_candidate。