cartographer代码学习笔记-构建多分辨率地图

构建多分辨率地图

1、配置参数阶段

FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options)
    : options_(options),
      limits_(grid.limits()),
      precomputation_grid_stack_(
          absl::make_unique<PrecomputationGridStack2D>(grid, options)) {}

初始配置参数阶段，FastCorrelativeScanMatcher2D类中初始化PrecomputationGridStack2D类对象precomputation_grid_stack_。

PrecomputationGridStack2D 类：

class PrecomputationGridStack2D {
 public:
  PrecomputationGridStack2D(
      const Grid2D& grid,
      const proto::FastCorrelativeScanMatcherOptions2D& options);

  const PrecomputationGrid2D& Get(int index) {
    return precomputation_grids_[index];
  }

  int max_depth() const { return precomputation_grids_.size() - 1; }

 private:
 //多分辨率地图 
  std::vector<PrecomputationGrid2D> precomputation_grids_;
}

2、构造多分辨率地图

通过for循环，根据深度设置对应分辨率地图：

PrecomputationGridStack2D::PrecomputationGridStack2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options) {
  CHECK_GE(options.branch_and_bound_depth(), 1);
  //分支定界深度，参数配置
  const int max_width = 1 << (options.branch_and_bound_depth() - 1);
  precomputation_grids_.reserve(options.branch_and_bound_depth());
  std::vector<float> reusable_intermediate_grid;
  const CellLimits limits = grid.limits().cell_limits();
  reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
                                     limits.num_y_cells);
  for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
    const int width = 1 << i;
    //调用PrecomputationGrid2D类成员函数构造多分辨率地图
    

precomputation_grids_.emplace_back(grid, limits, width,
                                       &reusable_intermediate_grid);

 
  }
}

每一层通过左移保证宽度为2的i次方

const int width = 1 << i;

i = 0   width = 1  原始地图
i = 1   width = 2  
i = 2   width = 4
i = 3   width = 8
i = 4   width = 16
i = 5   width = 32
i = 6   width = 64

3、PrecomputationGrid2D 函数

PrecomputationGrid2D::PrecomputationGrid2D(
    const Grid2D& grid, const CellLimits& limits, const int width,
    std::vector<float>* reusable_intermediate_grid)
    : offset_(-width + 1, -width + 1),
      wide_limits_(limits.num_x_cells + width - 1,
                   limits.num_y_cells + width - 1),
      min_score_(1.f - grid.GetMaxCorrespondenceCost()),
      max_score_(1.f - grid.GetMinCorrespondenceCost()),
      //cells_每个分辨率对应的单层地图
      cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
  CHECK_GE(width, 1);
  CHECK_GE(limits.num_x_cells, 1);
  CHECK_GE(limits.num_y_cells, 1);
  const int stride = wide_limits_.num_x_cells;
  // First we compute the maximum probability for each (x0, y) achieved in the
  // span defined by x0 <= x < x0 + width.
  //通过引用，给reusable_intermediate_grid赋值
  std::vector<float>& intermediate = *reusable_intermediate_grid;
  //重置reusable_intermediate_grid大小，x比y多了width - 1；
  intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
  for (int y = 0; y != limits.num_y_cells; ++y) {
  //通过滑动窗口计算占据情况：滑窗大小为width；
    SlidingWindowMaximum current_values; 
    首先获取每一列的第一个栅格的值放入到滑动窗口中
    current_values.AddValue(
        1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));
    for (int x = -width + 1; x != 0; ++x) {
    //沿着x轴方向遍历每一栅格，并将中间值设为该列最大值
      intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
      if (x + width < limits.num_x_cells) {
      //更新滑动窗口值
        current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
                                          Eigen::Array2i(x + width, y))));
      }
    }
    //超出滑动窗口大小
    for (int x = 0; x < limits.num_x_cells - width; ++x) {
    //中间值也设定为最大值
      intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
      //如果xy位置的值与窗口第一个一样就删掉第一个
      current_values.RemoveValue(
          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
      //滑动窗口新增一个与上一个x轴上相差width距离的值
      current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
                                        Eigen::Array2i(x + width, y))));
    }
    //滑动窗口超过后，删除前面的值，
    for (int x = std::max(limits.num_x_cells - width, 0);
         x != limits.num_x_cells; ++x) {
      intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
      current_values.RemoveValue(
          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
    }
    current_values.CheckIsEmpty();//保证滑窗内值为0
  }
  // For each (x, y), we compute the maximum probability in the width x width
  // region starting at each (x, y) and precompute the resulting bound on the
  // score.
  //同理，x轴遍历后，在x基础上遍历y轴，根据中间值计算滑窗值。
  for (int x = 0; x != wide_limits_.num_x_cells; ++x) {
    SlidingWindowMaximum current_values;
    current_values.AddValue(intermediate[x]);
    for (int y = -width + 1; y != 0; ++y) {
    //计算地图栅格概率值对应odd值，[0，255]区间
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      if (y + width < limits.num_y_cells) {
        current_values.AddValue(intermediate[x + (y + width) * stride]);
      }
    }
    //同样，滑窗size已满，需要删除一个再新增
    for (int y = 0; y < limits.num_y_cells - width; ++y) {
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      current_values.RemoveValue(intermediate[x + y * stride]);
      current_values.AddValue(intermediate[x + (y + width) * stride]);
    }
    //滑窗邻近结束，需要保证滑窗为空
    for (int y = std::max(limits.num_y_cells - width, 0);
         y != limits.num_y_cells; ++y) {
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      current_values.RemoveValue(intermediate[x + y * stride]);
    }
    current_values.CheckIsEmpty();
  }
}

代码理解参考: https://blog.youkuaiyun.com/weixin_43013761/article/details/131480496