(02)Cartographer源码无死角解析-(45) 2D栅格地图→ProbabilityGridRangeDataInserter2D

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过一系列的分析，对于 ActiveSubmaps2D、Submap2D、Submap、Grid2D 、ProbabilityGrid 的了解相对来说比较深刻了。不过目前存在如下两个疑问是待解决的：

$疑问1$→ 为什么Grid2D::FinishUpdate()函数中，栅格值为什么需要减去 kUpdateMarker？

$疑问3$ → ProbabilityGrid::ApplyLookupTable() 函数中对于cell的更新比较奇怪，代码为：*cell = table[*cell]。

也就是说，接下来分析的过程中需要注意 Grid2D::FinishUpdate() 与 ProbabilityGrid::ApplyLookupTable() 这两个函数的调用。虽然对 ActiveSubmaps2D、Submap2D、Submap、Grid2D 、ProbabilityGrid 都进行了具体的分析，那么这些类又是如何串联起来的？或者说他们是如何相互配合共同工作的呢？

这就是接下来需要讲解的内容了！把这些类都串起来的核心函数就是 ProbabilityGridRangeDataInserter2D::Insert()，在对其进行讲解之前来回顾一下前面的内容。函数间的调用关系如下：

LocalTrajectoryBuilder2D::AddRangeData() //添加点云数据
	LocalTrajectoryBuilder2D::AddAccumulatedRangeData()//添加累计，经过处理之后的点云数据
		LocalTrajectoryBuilder2D::InsertIntoSubmap() //将处理后的雷达数据写入submap

最总找到了 InsertIntoSubmap() 函数，其实现于 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 文件中，该函数就是 LocalTrajectoryBuilder2D 与 2D栅格地图的桥梁。

其核型功能是调用了 ActiveSubmaps2D::InsertRangeData() 函数，然后构建了 InsertionResult 格式的数据返回。其中 ActiveSubmaps2D::InsertRangeData() 函数在前面已经做过简单的讲解，主要核型代码如下：

AddSubmap(range_data.origin.head<2>()  //如果没有子图则回新建子图
submap->InsertRangeData(range_data, range_data_inserter_.get()); //将雷达数据插入到相邻的两个子图之中
submaps_.front()->Finish(); //如果第一个子图插入的点云帧数足够，则把该子图标记为完成状态

上述中，submap->InsertRangeData() 函数的核心是执行了如下代码：

range_data_inserter->Insert(range_data, grid_.get());

这里的 range_data_inserter，是在 ActiveSubmaps2D 构造函数的初始化列表调用 CreateRangeDataInserter() 函数创建的。CreateRangeDataInserter() 函数实现于 submap_2d.cc 文件中，

// 创建地图数据写入器
std::unique_ptr<RangeDataInserterInterface>
ActiveSubmaps2D::CreateRangeDataInserter() {
  switch (options_.range_data_inserter_options().range_data_inserter_type()) {
    // 概率栅格地图的写入器
    case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
      return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
          options_.range_data_inserter_options()
              .probability_grid_range_data_inserter_options_2d());
    // tsdf地图的写入器
    case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:
      return absl::make_unique<TSDFRangeDataInserter2D>(
          options_.range_data_inserter_options()
              .tsdf_range_data_inserter_options_2d());
    default:
      LOG(FATAL) << "Unknown RangeDataInserterType.";
  }
}

我们配置的是 PROBABILITY_GRID_INSERTER_2D，所以构建的是 ProbabilityGridRangeDataInserter2D 类对象，该类继承于 RangeDataInserterInterface。

总的来说，submap->InsertRangeData() 最终调用到的是 ProbabilityGridRangeDataInserter2D::Insert() 函数，该函数在 src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc 文件中实现。ProbabilityGridRangeDataInserter2D 存在三个私有成员变量,这里先记录一下：

 private:
  const proto::ProbabilityGridRangeDataInserterOptions2D options_;
  const std::vector<uint16> hit_table_;
  const std::vector<uint16> miss_table_;

 

二、疑问1解答

对于 $疑问1$ 与 $疑问3$ 的解答， 在 ProbabilityGridRangeDataInserter2D 的构造函数中可以找到答案

 

二、ProbabilityGridRangeDataInserter2D::Insert()

$(01)$ 利用c++多态机制，调用 static_cast 函数把参数 GridInterface* const grid 转换成 ProbabilityGrid 类型。

$(02)$ 调用 CastRays() 函数，该函数实现于 probability_grid_range_data_inserter_2d.cc 文件中。该函数是把点云数据帧插入到子图(或者说grid)的核心函数，后续回单独对齐进行详细的讲解。

$(03)$ 调用 Grid2D::FinishUpdate() 函数，该函数在前面已经讲解过，还留下了一个疑问。也就是疑问1→栅格值为什么需要减去 kUpdateMarker？

先来看一下代码，然后再来解答这个疑问：

/**
 * @brief 将点云写入栅格地图
 * 
 * @param[in] range_data 要写入地图的点云
 * @param[in] grid 栅格地图
 */
void ProbabilityGridRangeDataInserter2D::Insert(
    const sensor::RangeData& range_data, GridInterface* const grid) const {
  ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
  CHECK(probability_grid != nullptr);
  // By not finishing the update after hits are inserted, we give hits priority
  // (i.e. no hits will be ignored because of a miss in the same cell).
  // param: insert_free_space
  CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
           probability_grid);
  probability_grid->FinishUpdate();
}