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

一、前言

上一篇博客中，首先介绍了 ValueConversionTables，其最终的目的是生成一个转换表，该转换的主要的功能是把 $[0,1\backsim 32767]$ 的数值映射至 $[0.9,0.1\backsim 0.9]$。

虽然后重点讲解 Grid2D::GrowLimits() 函数，该函数主要功能就是判断传入的 point 是否位目前的子图之中，如果不在，则会把地图扩大至原来的四倍，直到 point 处于子图之中。地图的扩增思路如下图所示：

最后，还留下了如下两个疑问：

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

$疑问2$→ known_cells_box_ 是什么时候更新，哪里会发生变化，其作用是什么？

那么该篇博客，看下在 ProbabilityGrid 这个类中是否能够找到相关的答案。ProbabilityGrid 是 Grid2D 的派生类，该类声明于 src/cartographer/cartographer/mapping/2d/probability_grid.h 文件中，可见其成员变量还是比较简单的，仅仅一个转换表 ValueConversionTables* conversion_tables_ 而已。下面就来看看 probability_grid.cc 文件中其成员函数的实现。
 

二、ProbabilityGrid

1、ProbabilityGrid::ProbabilityGrid()

其存在两个构造函数，一个是根据参数 const MapLimits& limits 与 ValueConversionTables* conversion_tables 实例化对象，另外一个是根据 proto::Grid2D 的配置参数构造对象。但是过程都比较简单，主要就是构建了一个 Grid2D 对象，然后把转换表赋值给了成员变量 conversion_tables_。

第一个构造函数在调用父类构造函数的时候，传入了参数 kMinCorrespondenceCost 与 kMaxCorrespondenceCost，着两个值都为常量，定义如下：

constexpr float kMinProbability = 0.1f;                         // 0.1
constexpr float kMaxProbability = 1.f - kMinProbability;        // 0.9
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // 0.1
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // 0.9

kMinCorrespondenceCost 与 kMaxCorrespondenceCost 分别记录 Grid2D::correspondence_cost_cells_ 的最小值与最大值，当然指的是通过转换表映射之后的结果。
 

2、ProbabilityGrid::ProbabilityGrid()

从函数名可以直到，该函数的作用是设置栅格地图的概率概率值，第一个形参 cell_index 为 cell 的索引，第二个形参 probability 表示该 cell 被占用的机率。

其首先是通过 cell_index 获得该索引对应的cell，其是通过 mutable_correspondence_cost_cells() 函数获得，所以可以对该cell 的栅格值进行修改。不过由于 probability 表示该 cell 被占用的机率，所以这里调用了 ProbabilityToCorrespondenceCost() 函数，实际上就是 1-probability，获得未被占用的概率。然后再调用 CorrespondenceCostToValue() 函数，该函数的作用是把 $[0.9,0.1\backsim 0.9]$ 的数值映射至 $[0,1\backsim 32767]$，可以看做是转换表的逆操作。

因为对该 cell 被重新设置，说明其已经被探索到，即其是已知的了，所以通过 mutable_known_cells_box 函数把 cell 的二维索引(也可以看作是像素坐标)添加至 known_cells_box_ 之中。代码注释如下：

// Sets the probability of the cell at 'cell_index' to the given
// 'probability'. Only allowed if the cell was unknown before.
// 将 索引 处单元格的概率设置为给定的概率, 仅当单元格之前处于未知状态时才允许
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                     const float probability) {
  // 获取对应栅格的引用
  uint16& cell =
      (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
  CHECK_EQ(cell, kUnknownProbabilityValue);
  // 为栅格赋值 value
  cell =
      CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
}

 

3、ProbabilityGrid::ApplyLookupTable()

从该函数的命名来看，叫做应用查询表，那么其是如何应用的，且作用是什么呢？该函数的主要逻辑如下：

$(1)$ 其传入的参数 cell_index 与上一函数相同，通过该索引可以获得 correspondence_cost_cells_ 中对应的 cell。；另一参数 table 就是查询表，通过该查询表可以可以把 $[0,1\backsim 32767]$ 的数值映射到 $[0.9,0.1\backsim 0.9]$。

$(2)$ 该函数首先判断一下查询表 table 的大小，需要保证其为 kUpdateMarker=32768，否则报错。然后把二维 cell_index 变换成一维索引 flat_index。然后调用 mutable_correspondence_cost_cells() 函数，再借助 flat_index 获得其对应的 cell。判断一下 *cell >= kUpdateMarker 是否成立，如果成立表示该 cell 已经更新过了，无需再次更新，同时返回 false。

$(3)$ 该 cell 没有更新过，则通过 mutable_update_indices() 函数把 flat_index 添加到 update_indices_ 之中，表示该 cell 已经更新过了，然后再对其进行更新。更新的操作为 ： *cell = table[*cell]，其更新的操作比较怪→$疑问1$

$(4)$ 如果当前的 cell 更新了，说明该 cell 已经探索到，属于已知的 cell，通过 mutable_known_cells_box() 函数把其像素坐标添加到 known_cells_box_ 之中。

该函数的代码注释如下：

// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
// to the probability of the cell at 'cell_index' if the cell has not already
// been updated. Multiple updates of the same cell will be ignored until
// FinishUpdate() is called. Returns true if the cell was updated.
// 如果单元格尚未更新,则将调用 ComputeLookupTableToApplyOdds() 时指定的 'odds' 应用于单元格在 'cell_index' 处的概率
// 在调用 FinishUpdate() 之前，将忽略同一单元格的多次更新。如果单元格已更新，则返回 true
//
// If this is the first call to ApplyOdds() for the specified cell, its value
// will be set to probability corresponding to 'odds'.
// 如果这是对指定单元格第一次调用 ApplyOdds(),则其值将设置为与 'odds' 对应的概率

// 使用查找表对指定栅格进行栅格值的更新
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  const int flat_index = ToFlatIndex(cell_index);
  // 获取对应栅格的指针
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  // 对处于更新状态的栅格, 不再进行更新了
  if (*cell >= kUpdateMarker) {
    return false;
  }
  // 标记这个索引的栅格已经被更新过
  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;
}

该函数中，比较难理解的点 *cell = table[*cell]; 这句代码了，为了方便理解，打印了变量 flat_index、以及更新之前与更新之后的 *cell。如下：

flat_index           250643    ......    267468     249052  ......  250643   251439
*cell(更新之前)         0       ......    16794      16794   ......  14336    16794
*cell(更新之后)       47104     ......    47511      47511   ......  45097    47511

这里并没有看出什么，但是比较奇怪的是查询表并非从0开始的，如下图所示，确实比较出乎意料，不过没有关系，后续看下该函数是如何被调用的，然后再来分析出现该现象的原因。

 

5、ProbabilityGrid::GetProbability()

该函数较为简单，其就是传入一个cell索引值，