cartographer的概率栅格地图更新CastRays函数

cartographer的概率栅格地图更新CastRays函数

CastRays函数主要是通过调用ApplyLookupTable（）函数进行通过hit_table_ 与 miss_table_对hit 、miss更新

//probability_grid_range_data_inserter_2d.CC
void CastRays(const sensor::RangeData& range_data,
              const std::vector<uint16>& hit_table,
              const std::vector<uint16>& miss_table,
              const bool insert_free_space, ProbabilityGrid* probability_grid) {
  // 根据雷达数据调整地图范围
  GrowAsNeeded(range_data, probability_grid);

  const MapLimits& limits = probability_grid->limits();
  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
  const MapLimits superscaled_limits(
      superscaled_resolution, limits.max(),
      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                 limits.cell_limits().num_y_cells * kSubpixelScale));
  // 雷达原点在地图中的像素坐标, 作为画线的起始坐标
  const Eigen::Array2i begin =
      superscaled_limits.GetCellIndex(range_data.origin.head<2>());
  // Compute and add the end points.
  std::vector<Eigen::Array2i> ends;
  ends.reserve(range_data.returns.size());
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
    // 计算hit点在地图中的像素坐标, 作为画线的终止点坐标
    ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
    // 更新hit点的栅格值
    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
  }
  
  // 如果不插入free空间就可以结束了
  if (!insert_free_space) {
    return;
  }

  // Now add the misses.
  for (const Eigen::Array2i& end : ends) {
    std::vector<Eigen::Array2i> ray =
        RayToPixelMask(begin, end, kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
      // 从起点到end点之前, 更新miss点的栅格值
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }

  // Finally, compute and add empty rays based on misses in the range data.
  for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
    std::vector<Eigen::Array2i> ray = RayToPixelMask(
        begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
        kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
      // 从起点到misses点之前, 更新miss点的栅格值
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }
}
}  // namespace

首先分析更新hit点的栅格值 probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
对于更新是有规则的：一帧点云数据，某个cell只更新一次；
把将要cell的索引放在mutable_update_indices
cell被更新后，*cell大于等于 kUpdateMarker，如果此帧点云又判断出要更新cell，但发现值已>=kUpdateMarker，认为此轮已更新过，不执行操作。更新完了此帧点云，FinishUpdate（）将update_indices_集合中的栅格值减去kUpdateMarker。

//probability_grid.cc
// 使用查找表对指定栅格进行栅格值的更新
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table) {
  //更新表的大小固定，    kUpdateMarker = 32768                                 
  DCHECK_EQ(table.size(), kUpdateMarker);
  const int flat_index = ToFlatIndex(cell_index);
  // 获取对应栅格的指针，就是根据索引找打对应的correspondence_cost值
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  //cell初始值为0（kUnknownCorrespondenceValue），被更新一次之后，cell对应的值就会大于kUpdateMarker， 对处于更新状态的栅格, 不再进行更新了，防止一个cell，一帧被更新多次
  if (*cell >= kUpdateMarker) {
    return false;
  }
  // 那什么时候 小于 kUpdateMaker？——更新完此帧点云后，见void Grid2D::FinishUpdate()，该函数会减去kUpdateMarker
  //执行这一步，说明cell马上要更新了，先标记这个索引的栅格已经被更新过
  mutable_update_indices()->push_back(flat_index);
  // 更新栅格值
  *cell = table[*cell];
  DCHECK_GE(*cell, kUpdateMarker);
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
  return true;
}

欢迎大家一起学习交流~

每天进步一点点，时光不负有心人❤