cartographer多分辨率地图生成

class PrecomputationGridStack2D
该类在构造FastCorrelativeScanMatcher2D类时被构造:会有几个不同分辨率的栅格地图,这个类被PrecomputationGridStack引用.

 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_;
};

最粗的分辨率:是由分枝定界的深度决定的。

  CHECK_GE(options.branch_and_bound_depth(), 1);
  const int max_width = 1 << (options.branch_and_bound_depth() - 1);

GridStack中地图的个数 即不同分辨率地图的个数

  precomputation_grids_.reserve(options.branch_and_bound_depth());

可以重复使用的中间栅格

    std::vector<float> reusable_intermediate_grid;
    const CellLimits limits = probability_grid.limits().cell_limits();
 
    reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
                                       limits.num_y_cells);

构造各个不同分辨率的栅格地图 width表示不同分辨率的栅格,最原始的地图是最细分辨率的栅格地图

  for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
    const int width = 1 << i;
    precomputation_grids_.emplace_back(grid, limits, width,
                                       &reusable_intermediate_grid);
  }

取地图的高度

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

获取最大深度

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

class PrecomputationGrid2D
这个类表示栅格地图，这个栅格地图的作用是通过预先计算网格来加速Multi-Level Resolution.
构造函数

  PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, 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_(wide_limits_.num_x_cells * wide_limits_.num_y_cells)

这个函数是在原始地图的基础上 生成分辨率为width*origin_resolution的地图.

* @param grid                  对应的概率地图(原始地图)
* @param limits                地图的参数(包括x和y方向上的cell的数量)
* @param width                 可以认为是地图的分辨率 width*width个原始地图栅格合成一个粗分辨率的栅格
* @param reusable_intermediate_grid          可以重复使用的中间栅格 用来保存最大值的一个中间变量

滑动窗口的长度是width,所以

wide_limits_(limits.num_x_cells + width - 1,limits.num_y_cells + width - 1),

地图的一行的栅格数量

  const int stride = wide_limits_.num_x_cells;

中间变量的大小为加入滑动窗口后地图的大小

intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);

枚举原始地图所有的y 即所有的行,把每一行分为一个一个的长度为width段(即长度为width的滑动窗口)，求解出每个width段的最大值,保存到intermediate里

for (int y = 0; y != limits.num_y_cells; ++y) { //遍历每一行
    SlidingWindowMaximum current_values;
    current_values.AddValue(
        1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));//每一行第一个栅格的值
    for (int x = -width + 1; x != 0; ++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();
      current_values.RemoveValue(
          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
      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) {//滑动窗口要离开该行时,每往右滑动一次,删除一个值,直至完全滑出,滑动窗口里的值的个数为0
      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();
  }

当遍历完每一行的时候,就实现了将width长的栅格(一个width包含多个栅格)值变成了一样(即多个栅格中的最大值),即行方向上的合并,接下来是列方向上的合并

 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) {
    //窗口未满的时候
      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]);
      }
    }
    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) {//滑动窗口要离开该列时,每往上滑动一次,删除一个值,直至完全滑出,滑动窗口里的值的个数为0
      cells_[x + (y + width - 1) * stride] =
          ComputeCellValue(current_values.GetMaximum());
      current_values.RemoveValue(intermediate[x + y * stride]);
    }
    current_values.CheckIsEmpty();
  }

当遍历完列后,就实现了行和列上的栅格的合并,即生成了width分辨率的地图,值得注意的是,原始地图一直是不变的,例如,width取2,4,8.生成widthorigin_resolution分辨率地图时,都是在原始地图上生成的,即4origin_resolution分辨率地图是在origin_resolution分辨率地图上生成的而不是在2*origin_resolution分辨率地图上生成的.

参考:
https://blog.csdn.net/xiaoma_bk/article/details/83040559