(02) Cartographer source code analysis without dead ends - (67) 2D backend optimization → FastCorrelativeScanMatcher2D - branch and bound algorithm (BranchAndBound) 1

Explain the summary link of a series of articles about slam: the most complete slam in history starts from scratch , and explain for this column (02) Analysis of Cartographer source code without dead ends-links are as follows:
(02) Cartographer source code analysis without dead ends- (00) Catalog_Latest None Dead corner explanation: https://blog.csdn.net/weixin_43013761/article/details/127350885

$certification{\color{blue}{right below the end of the article Center} provides my \color{red} contact information, \color{blue} clicks on my photo to display WX→official certification}$

I. Introduction

In the previous blog, a detailed analysis of PrecomputationGrid and PrecomputationGridStack2D was carried out. Its main function is to build multi-resolution low, and at the end we mentioned that the higher the level stored in PrecomputationGridStack2D, the higher the resolution. . The main purpose of this blog is to achieve the following:
insert image description here estimate the pose of the robot (node in) graph gt from Figure 1, Figure 2, and Figure 3. Here it is assumed that the parameter branch_and_bound_depth=3 in the .lua file is explained as an example. Figure 1, Figure 2, and Figure 3 on it are regarded as the multi-resolution maps solved by the previous blog.

As shown in Figure 3, first use the coarsest resolution, where the pose of the robot is considered as an initial pose, which is recorded here as $\mathbf {Robot3}^{local}_{tracking}$ , obviously, based on this relatively coarse resolution map, it is easy to match, but it is very imprecise. Because its corresponding width=4, as shown in the figure below, 4x4 pixels will be treated as 1 pixel. As shown in the figure below, I only drew a part, that is, the blue and yellow areas: Next, pay attention to
insert image description here
: $\ color{red}Note:$ It can be seen that the pose of the robot falls in the yellow area under the local system, at the 4 points [(0,1), (0,2), (1,1), (1,2)] (from 0 Start), perform violent correlation scanning and matching, and can get a good optimal solution, which we record as the optimal candidate circle. Next, we find the corresponding area in Figure 2. It should be noted that width=2 in Figure 2, so It will treat 2x2=4 pixels as 1 pixel, as follows:
insert image description here
First, carry out violent correlation scanning and matching on [2:5,0:3] in the appeal, that is, the area inside the green rectangle, and finally find four The candidate solution, that is, the yellow area, the coordinates of the four points are [(2,2), (2,3), (3,2), (3,3)] respectively. In the same way, find the areas corresponding to these four points in Figure 1, and then perform violent correlation scanning and matching, as follows:
insert image description here
Through the violent correlation scanning and matching of the area inside the green rectangular frame, four good solutions can finally be obtained , but this time we can directly choose the highest one. That is the final solution. The optimal solution in the above figure is very close to the solution of the front figure gt.

Here we use diagrams to explain. Although it is easier to understand, it is not easy to connect with nouns such as tree, branch, tree depth, and branch. After analyzing the source code, let's compare it in a tree form, and everyone will understand what's going on, and why this algorithm is called the branch and bound algorithm.

二、::FastCorrelativeScanMatcher2D()

In the file fast_correlative_scan_matcher_2d.h, you can see the name of the FastCorrelativeScanMatcher2D class. Its member variables are very simple, so skip it and start directly from the constructor of FastCorrelativeScanMatcher2D, but its constructor is also very simple, as shown below:

// 构造函数
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options)
    : options_(options),
      limits_(grid.limits()),
      // 多分辨率地图的构建
      precomputation_grid_stack_(
          absl::make_unique<PrecomputationGridStack2D>(grid, options)) {
    
    }

It is to build a multi-resolution map according to the parameter configuration and grid, that is, the PrecomputationGridStack2D instance object is assigned to precomputation_grid_stack_.

3. ::Match() and SearchParameters

This function mainly calls the MatchWithSearchParameters() function, as follows:

/**
 * @brief 进行局部搜索窗口的约束计算(对局部子图进行回环检测)
 * 
 * @param[in] initial_pose_estimate 先验位姿
 * @param[in] point_cloud 原点位于local坐标系原点处的点云
 * @param[in] min_score 最小阈值, 低于这个分数会返回失败
 * @param[out] score 匹配后的得分
 * @param[out] pose_estimate 匹配后得到的位姿
 * @return true 匹配成功, 反之匹配失败
 */
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 {
    
    
  // param: linear_search_window angular_search_window 
  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);
}

First, initial_pose_estimate represents the initial pose of the robot (node), or the pose to be optimized. point_cloud should be the point cloud data under the local system. min_score is a threshold value, and the subsequent calculated pose solution, as long as the score is greater than the threshold value, is regarded as the most candidate solution. Regarding the SearchParameters class, there is a very slender introduction in the front, as shown in the link below.

(02) Cartographer source code analysis without dead ends - (49) 2D point cloud scan matching → correlation violence matching 1: SearchParameters

There are several important functions in this class, namely SearchParameters::GenerateRotatedScans() and DiscretizeScans(), which are used to traverse the angle and position on the map respectively. There is also the function SearchParameters::ShrinkToFit(). The previous blogs have not been used, so they have not been analyzed. Let’s take a look now:

// 计算每一帧点云 在保证最后一个点能在地图范围内时 的最大移动范围
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();

    // 对点云的每一个点进行遍历, 确定这帧点云的最大最小的坐标索引
    for (const Eigen::Array2i& xy_index : scans[i]) {
    
    
      // Array2i.min的作用是 获取对应元素的最小值组成新的Array2i
      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);
    }

    // 计算每一帧点云 在保证最后一个点能在地图范围内时 的最大移动范围
    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());
  }
}

scans contains multiple frames of point cloud data. This function traverses each frame of point cloud, and then judges the maximum distance that a frame of point cloud can move in 4 directions on the map. After moving the point cloud data according to this distance, it is guaranteed that at least There is a point cloud data located behind the map, otherwise it is meaningless, and the point cloud goes outside the map during traversal.

After the ::Match() function constructs the SearchParameters instance search_parameters, it then calls the ::MatchWithSearchParameters() function and passes in search_parameters as an actual parameter.

四、::MatchWithSearchParameters()

$\color{blue}（1）：$ It is not difficult to see from the name that this function is to scan and match according to the SearchParameters object. First look at the input to the function:

// 进行基于分支定界算法的粗匹配
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(score != nullptr);
  CHECK(pose_estimate != nullptr);

This function accepts a SearchParameters instance object, an initial pose initial_pose_estimate, and point cloud data for violent search. The point cloud data here should still be in the local system, and min_score is used to filter some better pose solutions . score and pose_estimate are used to store the score and pose after matching respectively.

$\color{blue}（2）：$ Then the point cloud data is rotated, and the angle of the point cloud is relative to the robot (node) coordinate system. Then call the GenerateRotatedScans function to generate each point cloud data that needs to traverse the angle, which is essentially to rotate the point cloud data.

  // Step: 将原点处的点云先旋转到预测的方向上
  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())));

  // Step: 生成按照不同角度旋转后的点云集合
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

$\color{blue}（3）：$ Translate the rotated point cloud frame set, and the point cloud data after translation is relative to the robot (node) coordinate system regardless of rotation or translation. Then or the index of these point cloud data on the map, as follows

  // Step: 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引
  // 这里的离散激光点是在最细的分辨率的地图上面
  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());

Then call the search_parameters.ShrinkToFit() function to calculate the maximum movement range of each frame point cloud when ensuring that the last point can be within the range of the map, in the search_parameters.linear_bounds variable.

$\color{blue}（4）：$ The reporter calls the ComputeLowestResolutionCandidates() function to obtain all qualified candidate solutions for the map with the lowest resolution (thickest obstacle). The specific details will be discussed later.

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

$\color{blue}（5）：$ After the initial batch of candidate solutions, the branch and bound algorithm can be called, that is, the BranchAndBound() function, the code is as follows:

  // Step: 进行基于分支定界算法的搜索, 获取最优解
  const Candidate2D best_candidate = BranchAndBound(
      discrete_scans, search_parameters, lowest_resolution_candidates,
      precomputation_grid_stack_->max_depth(), min_score); // param: max_depth
  
  // 检查最优解的值, 如果大于指定阈值min_score就认为匹配成功,否则认为不匹配返回失败
  if (best_candidate.score > min_score) {
    
    
    *score = best_candidate.score;
    // Step: 根据计算出的偏移量对位姿进行校准
    *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;
  }

For the time being, BranchAndBound() will not be analyzed in detail, and will be explained separately later. Finally, judge whether the optimal solution exceeds the specified threshold min_score. If so, adjust the initial_pose_estimate to obtain the best pose.

五、::ComputeLowestResolutionCandidates()

Now go back and analyze the code ComputeLowestResolutionCandidates. The code is actually relatively easy to understand. As mentioned earlier, the map with the lowest resolution in the source code:

precomputation_grids_[6]       width=64      可以简单理解，比原始地图构建的分辨率地图
增加x,y增加64-1=1个像素，且每 64x64=4096 个像素的像素值都用他们中最大的来代替(略有区别)

Treat 4096 pixels as a new pixel and process a sub-image. Even if you traverse all the new pixels of the sub-image, it will not consume much resources. Look at the source code as follows:

// 生成最低分辨率层(栅格最粗)上的所有候选解, 并进行打分与排序
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters) const {
    
    

  // 生成最低分辨率层(栅格最粗)上的所有候选解
  std::vector<Candidate2D> lowest_resolution_candidates =
      GenerateLowestResolutionCandidates(search_parameters);

  // 计算每个候选解的得分, 按照匹配得分从大到小排序, 返回排列好的candidates 
  ScoreCandidates(
      precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
      discrete_scans, search_parameters, &lowest_resolution_candidates);
  return lowest_resolution_candidates;
}

It seems that the logic is very simple. First call the GenerateLowestResolutionCandidates() function to calculate the maximum distance that the point cloud data moves in the map according to the search_parameters to find all relevant candidate solutions. Then sort in descending order by the ScoreCandidates() function. The code for the GenerateLowestResolutionCandidates() function is not very complicated, as follows:

// 生成最低分辨率层(栅格最粗)上的所有候选解
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
    const SearchParameters& search_parameters) const {
    
    
  const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
  int num_candidates = 0;
  // 遍历旋转后的每个点云
  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;
  }

  // 将所有候选解保存起来, 候选解的结构为（角度的索引, 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;
}

It first loops through the point cloud, which is simply understood as angle traversal. In the loop process, there is a parameter, which is linear_step_size, which is equivalent to the aforementioned width=64, that is, 64x64=4096 is treated as a pixel. In the process of traversal, search_parameters.linear_bounds is used, and each frame of point cloud traversed must belong to its own linear_bound, so as to ensure that at least one point cloud of all candidate solution point cloud frames is in the map.

Subsequent operations need to be noted that the candidate solution stores the offset relative to the origin coordinates of the initial point cloud (robot system), not an absolute coordinate.

6. Conclusion

This blog shows the core principle of the branch and bound algorithm visually and intuitively through illustrations first, and explains the process of obtaining candidate solutions with the lowest resolution in the follow-up process, and arranges the candidate solutions in descending order. However, the core algorithm part of branch and bound, namely FastCorrelativeScanMatcher2D::BranchAndBound(), has not been explained yet. This is the main content of the next blog.