(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与疑问3引入

对于 $疑问1$ 与 $疑问3$ 的解答， 在 ProbabilityGridRangeDataInserter2D 的构造函数中可以找到答案，先来看看 ProbabilityGridRangeDataInserter2D.cc 文件中的构造函数：

// 写入器的构造, 新建了2个查找表
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
    const proto::ProbabilityGridRangeDataInserterOptions2D& options)
    : options_(options),
      // 生成更新占用栅格时的查找表 // param: hit_probability
      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.hit_probability()))),    // 0.55
      // 生成更新空闲栅格时的查找表 // param: miss_probability
      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.miss_probability()))) {
    
    } // 0.49

从这里来看，可以知道其构造函数主要就是根据 hit_probability 与 miss_probability 创建了两个查询表 hit_table_ 与 miss_table_。其都是调用 ComputeLookupTableToApplyCorrespondenceCostOdds() 函数。该函数实现于 src/cartographer/cartographer/mapping/probability_values.cc 文件中。

$提示$ 为了大家透彻的理解该函数，这里先说一下该函数的目的。首先，该函数会返回一个更新查询表，该表有什么作用呢？前面分析过程中，已经知道 ProbabilityGrid::ApplyLookupTable() 函数中，会对cell 进行更新，实际上也就是对 correspondence_cost_cells_ 的更新。

那么是如何更新的？在疑问3提到更新的代码 *cell = table[*cell] 比较怪异，不是很明白。起始呢，算法的原理是这样的，先来看论文中更新的公式：
$$M_{\text{new }}(x)=\mathrm{clamp}\left({\text{ odds }}^{-1}\left(\text{ odds }\left(M_{\text{old }}(x)\right)\cdot \text{ odds }\left(p_{\text{hit }}\right)\right)\right)\text{\hspace{1em}(01)}$$其上的 这里这里以 hit_probability=options.hit_probability()=0.55 为例，那么上式中的 $p_{\text{hit}}$=0.55， $M_{\text{old}}(x)$ 表示 cell 未更新之前的被占用的概率值，$M_{\text{new }}(x)$ 表示更新之后的 cell 被占用的概率值。${\mathrm{odds}}^{-1}$ 表示的就是 $\mathrm{odds}$ 的逆操作，在上一篇博客中提到 $\mathrm{odds}$ 表示被占用概率与未被占用概率的比值，论文中的公式如下：$$\mathrm{odds}(p)=\frac{p}{1-p}\text{\hspace{1em}(02)}$$关于 $\mathrm{odds}$ 与 ${\mathrm{odds}}^{-1}$ 源码实现如下：

// 通过概率计算Odd值 论文里的 odds(p)函数
inline float Odds(float probability) {
    
    
  return probability / (1.f - probability);
}

// 通过Odd值计算概率值 论文里的 odds^-1 函数
inline float ProbabilityFromOdds(const float odds) {
    
    
  return odds / (odds + 1.f);
}

现在再回过头来看公式(01)就比较好理解了，因为 odds 越大，表示cell被占用的概率越大。如果cell被占用的概率与未被占用的概率都为0.5相等时，odds 为1。如果cell未被占用的机率很大很大，那么 odds 是趋向于0的一个数。也就是说，原始 cell 对应的 $\text{ odds }(M_{\text{old }}(x))$ 乘以 hit_probability=0.55 对应的$\text{ odds }(p_{\text{hit }})>1$，那么其结果 $M_{\text{new }}(x)$ 肯定是增加的，从而达到更新的效果。

$总结$ 总的来说，使用 hit_probability=0.55>0.5 对 cell 进行更新，则 cell 对应被占用的概率越来越大。如果使用 miss_probability=options.miss_probability()=0.49<0.5 对 cell 进行更新，则 cell 对应被占用的概率越来越小。

这里的确了解了cell更新的原理，但是似乎并没有解答疑问1与疑问3。虽然论文中的公式是这样的，但是源码并非是这样的实现的。下面就从源码的角度来解答 $疑问1$ 与 $疑问3$ 吧。
 

三、ComputeLookupTableToApplyCorrespondenceCostOdds()

从上面的分析中，知道 ProbabilityGridRangeDataInserter2D 的核型是调用了 ComputeLookupTableToApplyCorrespondenceCostOdds函数。根据上面公式虽然可以对 cell 的栅格值进行更新，但是每次计算都涉及到了重复的乘除法计算。所以，Cartographer 源码实现过程中，又使用到了用空间换效率的方式。

目的不是要对 cell 进行更新吗？那么是不是可以提前计算出 cell 更新的所有可能值呢？答案是可以的，因为 cell 就是[0,32768] 中的一个数值，那么把 [0,32768] 跟新后的所有值都保存下来即可。源码中就是这样做的，具体实现方式如下：

$(01)$ 首先创建一个 std::vector<uint16> result 变量，用于保存结果，这里的结果就是转换表。也就是 cell 更行之后所有可能的取值。

$(02)$ result 存储第一个元素比较特殊，其存储的是形参odds对应的栅格值，这里可以理解为把形参 odds 以 uint16 的方式进行存储。不过注意，再存储之时，该值还加上了 kUpdateMarker=32768。

$(03)$ 计算cell更新时从1到32768的所有可能更新后的结果，然后存储于 result 之中，源码中的Odds(CorrespondenceCostToProbability( (*kValueToCorrespondenceCost)[cell]))等价于公式(1)的 $\text{odds }(M_{\text{old }}(x))$，源码中的形参 odds 等价于公式(1)中的 $\text{odds }(p_{\text{hit}})$。

源代码的注释如下：

// 将栅格是未知状态与odds状态下, 将更新时的所有可能结果预先计算出来
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
    float odds) {
    
    
  std::vector<uint16> result;
  result.reserve(kValueCount); // 32768

  // 当前cell是unknown情况下直接把odds转成value存进来
  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
                       ProbabilityFromOdds(odds))) +
                   kUpdateMarker); // 加上kUpdateMarker作为一个标志, 代表这个栅格已经被更新了
  // 计算更新时 从1到32768的所有可能的 更新后的结果 
  for (int cell = 1; cell != kValueCount; ++cell) {
    
    
    result.push_back(
        CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                odds * Odds(CorrespondenceCostToProbability(
                           (*kValueToCorrespondenceCost)[cell]))))) +
        kUpdateMarker);
  }
  return result;
}

根据上面分析，可以知道，根据返回的更行查询表 result，可以对 cell 进行更行，这里更新的时候 +kUpdateMarker，所以后面后续再调用 Grid2D::FinishUpdate()函数中，cell 栅格需要减去 kUpdateMarker，以复原。这样就对 $疑问1$ 进行了解答。对于 $疑问3$ 的解答，先不要着急，我们先来来看看 ProbabilityGridRangeDataInserter2D::Insert() 函数。
 

四、ProbabilityGridRangeDataInserter2D::Insert()

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

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

$(03)$ 调用 Grid2D::FinishUpdate() 函数，根据前面的分析我们知道每个跟新过的 cell 都需要减去 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();
}

五、probability_grid->ApplyLookupTable()

上面通过对 ComputeLookupTableToApplyCorrespondenceCostOdds() 了，解答了疑问1，现在还剩下 $疑问3$ → ProbabilityGrid::ApplyLookupTable() 函数中对于cell的更新比较奇怪，代码为：*cell = table[*cell]。

该函数被 src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc 文件中的 CastRays() 函数调用，且被调用了三次。这里先不对 CastRays() 函数进行具体分析，下一篇博客再进行详细的讲解。其三次调用 probability_grid->ApplyLookupTable() 的代码如下所示：

    // 更新hit点的栅格值
    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
    
    // 从起点到end点之前, 更新miss点的栅格值
    probability_grid->ApplyLookupTable(cell_index, miss_table);

    // 从起点到misses点之前, 更新miss点的栅格值
    probability_grid->ApplyLookupTable(cell_index, miss_table);

从这里可以看出，其主要是根据 hit_table 或者 miss_table 这两个更新查询表对 cell 进行更新。这里需要注意的是，使用 hit_table 更新，其 cell 对应被占用的概率越大。使用 miss_table 更新，其其 cell 对应被占用的概率越小，即未被占用的概率越大。其原因是因为构建 hit_table_ 与 miss_table_ 的参数分别是 options.hit_probability() 与 options.miss_probability()，前者大于0.5，后者小于0.5。

那么问题3的答案就出现了，根据前面的分析: hit_table_ 与 miss_table_ 的索引就是代表未更新之前的cell，索引对应位置存储的就是更新后的 cell，所以通过执行代码 *cell = table[*cell]。即可完成对cell 的更新。
 

五、结语

通过该篇博客，我们解答了以下两个疑问，同时对 ProbabilityGridRangeDataInserter2D 的主要成员函数进行了分析。

$疑问1$→ 为什么Grid2D::FinishUpdate()函数中，栅格值为什么需要减去 kUpdateMarker？
$疑问3$ → ProbabilityGrid::ApplyLookupTable() 函数中对于cell的更新比较奇怪，代码为：*cell = table[*cell]。

疑问1是因为cell更新时加上了kUpdateMarker，所以再完成更新时，需要减去该值，以复原。疑问3是因为，hit_table_ 与 miss_table_ 的索引就是代表未更新之前的cell，索引对应位置存储的就是更新后的 cell。

另外，对于 src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc 文件中的 CastRays() 函数还没有进行详细的讲解。其就是下一篇博客需要讲解的内容了。