算法|Cartographer中的branch and bound算法的理解算法|机器人

target：寻找从scan坐标系到submap坐标系的最佳变换参数构建Candidate树
【算法|Cartographer中的branch and bound算法的理解】搜索空间由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。