(02)Cartographer源码无死角解析-(44) 2D栅格地图→ProbabilityGrid 与 ProbabilityToLogOddsInteger()

讲解关于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]，其更新的操作比较怪→$疑问3$

$(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开始的，如下图所示，确实比较出乎意料，不过没有关系，后续看下该函数是如何被调用的，然后再来分析出现该现象的原因。

 

4、ProbabilityGrid::GetProbability()

该函数较为简单，其就是传入一个cell索引值，然后获得该 cell 被占用的概率。通过 correspondence_cost_cells() 函数，结合 cell 一维索引获得的是 cell 对应的未被占用的栅格值，其范围是 $[0,1\backsim 32767]$，所以还要调用 ValueToCorrespondenceCost() 函数，把其映射到 $[0.9,0.1\backsim 0.9]$ 上。映射之后的概率表示为被占用的概率，所用最后还调用了 CorrespondenceCostToProbability() 把其转换成 cell 被占用的概率。

// Returns the probability of the cell with 'cell_index'.
// 获取 索引 处单元格的占用概率
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
    
    
  if (!limits().Contains(cell_index)) return kMinProbability;
  return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
      correspondence_cost_cells()[ToFlatIndex(cell_index)]));
}

这里额外来看一下 install_isolated/include/cartographer/mapping/probability_values.h 文件中的图下代码：

// c++11: extern c风格
extern const std::vector<float>* const kValueToProbability;
extern const std::vector<float>* const kValueToCorrespondenceCost;

// Converts a uint16 (which may or may not have the update marker set) to a
// probability in the range [kMinProbability, kMaxProbability].
inline float ValueToProbability(const uint16 value) {
    
    
  return (*kValueToProbability)[value];
}

// Converts a uint16 (which may or may not have the update marker set) to a
// correspondence cost in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ValueToCorrespondenceCost(const uint16 value) {
    
    
  return (*kValueToCorrespondenceCost)[value];
}

其上的 kValueToProbability 与 kValueToCorrespondenceCost 是两个转换表，第一个表示转换成被占用的概率，第二个表示未被占用的概率。这两个转换表在 src/cartographer/cartographer/mapping/probability_values.cc 文件中定义，由兴趣的朋友可以具体了解一下，代码与之前生成转换表的原理一致。
 

5、ProbabilityGrid::ComputeCroppedGrid()

通过前面一系列的分析，有了知识储备之后，再来分析该函数就比较简单了。在前面提到过，可以把子图看作一个 Grid，从函数名可以看出，该函数的主要目的就是对 Grid 进行剪切，那么如何剪切呢？无论其如何剪切，都必须保证已经探索过的 cell 被保留下来，源码的主体流程如下：

$(1)$ 首先调用 ComputeCroppedLimits() 函数，然后把 offset 与 cell_limits 的地址作为实参进行传递。该函数是在父类 Grid2D 中实现的，前面已经讲解过，其主要是获得 known_cells_box_ 最小值(xy)，即其高宽。简单的说，就把 known_cells_box_ 看作 Grid(或子图) 上的一个框，这个框包含了所有目前探索过 cell 对应的像素坐标。

$(2)$ 通过 ComputeCroppedLimits() 函数后，offset 与 limits 共同描述了 known_cells_box_ 的区域， 根据该区域重新定义地图。offset 表示旧栅格地图原点(左上角)到 known_cells_box_ 原点(左上角)的偏移值，新地图首先要减去该偏移值，然后使用 cell_limits 定义新地图的栅格数。这些就获得了剪切之后的新地图 cropped_grid。

$(3)$ 通过 GetProbability(xy_index + offset) 函数获取旧地图 cell 被占用的概率，然后赋值给新地图 cropped_grid，本质上是对 cropped_grid::correspondence_cost_cells_ 变量进行操作。最后返回新地图 cropped_grid 的智能指针。

代码注释如下：

// 根据bounding_box对栅格地图进行裁剪到正好包含点云
std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
    
    
  Eigen::Array2i offset;
  CellLimits cell_limits;
  // 根据bounding_box对栅格地图进行裁剪
  ComputeCroppedLimits(&offset, &cell_limits);
  const double resolution = limits().resolution();
  // 重新计算最大值坐标
  const Eigen::Vector2d max =
      limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
  // 重新定义概率栅格地图的大小
  std::unique_ptr<ProbabilityGrid> cropped_grid =
      absl::make_unique<ProbabilityGrid>(
          MapLimits(resolution, max, cell_limits), conversion_tables_);
  // 给新栅格地图赋值
  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
    
    
    if (!IsKnown(xy_index + offset)) continue;
    cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
  }

  // 返回新地图的指针
  return std::unique_ptr<Grid2D>(cropped_grid.release());
}

6、ProbabilityGrid::DrawToSubmapTexture()

从函数命名来看，其功能是在子图上绘画文本。关于地图栅格数据的存储，使用 std::string 的形式，源码中创建了变量
std::string cells。其每2字节描述描述一个cell：

①第一个字节→栅格值value
②第二个字节→alpha透明度

$(1)$ 调用父类 ComputeCroppedLimits(&offset, &cell_limits) 函数，对栅格地图进行剪裁。创建一个 std::string 对象 cells。然后对剪切之后地图所有cell进行遍历。

$(2)$ 如果根据 known_cells_box_ 剪切出来的 cell 不在原地图之中，则把该 cell 的两个字节都设置为0，表示未知。

$(3)$ 如果根据 known_cells_box_ 剪切出来的 cell 在原地图之中，那么首先获取该cell被占用的概率值，通过 ProbabilityToLogOddsInteger() 函数把其映射到 [1, 255] 之间，然后再映射到[-127, 127]，使用变量 delta 表示。关于 ProbabilityToLogOddsInteger() 函数后面单独分析。

$(4)$ delta 的范围在 [-127, 127] 之间，总的来说 delta>0时，越接近127，表示 cell 被占用的机率越大。总的来说 delta<0时，越接近-127，表示 cell 没有被占用的机率越大。

$(5)$ ①→当 delta > 0 时，alpha=0，value = delta。     ~~~    ②→当 delta <= 0 时，alpha=-delta，value=0。
从这里可以看出当 delta > 0 时, 其直接被设置为栅格值value，delta > 0 时, 栅格值value被设置为0。对于透明度alpha，当 delta > 0 时，alpha=0，表示不透明。当 delta <= 0 时，透明度设置为 -delta。也就是说最总透明度 alpha 使用一个正值来表示，越接近 127，则透明度越高。

$(6)$ 总的来说当栅格cell被占用时，value>0，alpha=0。当栅格cell未被占用时，value=0，alpha>0。另外如果 value=alpha=0时，透明度会被设置成1。

$(7)$ 把所有cell的栅格信息以 value 与 alpha 的形式存储到 std::string cells 之后，会调用 common::FastGzipString 对栅格地图数据进行压缩，压缩结果存储于 texture 之中。

$(8)$ 填充地图信息，如宽高cell数，分辨率等等最后调用了 *texture->mutable_slice_pose() 函数进行赋值，这里没有看明白，先记一下。

关于 ProbabilityGrid::DrawToSubmapTexture() 函数的注释如下:

// 获取压缩后的地图栅格数据
bool ProbabilityGrid::DrawToSubmapTexture(
    proto::SubmapQuery::Response::SubmapTexture* const texture,
    transform::Rigid3d local_pose) const {
    
    
  Eigen::Array2i offset;
  CellLimits cell_limits;
  // 根据bounding_box对栅格地图进行裁剪
  ComputeCroppedLimits(&offset, &cell_limits);

  std::string cells;
  // 遍历地图, 将栅格数据存入cells
  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
    
    
    if (!IsKnown(xy_index + offset)) {
    
    
      cells.push_back(0 /* unknown log odds value */);
      cells.push_back(0 /* alpha */);
      continue;
    }
    // We would like to add 'delta' but this is not possible using a value and
    // alpha. We use premultiplied alpha, so when 'delta' is positive we can
    // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
    // zero, and use 'alpha' to subtract. This is only correct when the pixel
    // is currently white, so walls will look too gray. This should be hard to
    // detect visually for the user, though.
    // 我们想添加 'delta'，但使用值和 alpha 是不可能的
    // 我们使用预乘 alpha，因此当 'delta' 为正时，我们可以通过将 'alpha' 设置为零来添加它。 
    // 如果它是负数，我们将 'value' 设置为零，并使用 'alpha' 进行减法。 这仅在像素当前为白色时才正确，因此墙壁看起来太灰。 
    // 但是，这对于用户来说应该很难在视觉上检测到。
    
    // delta处于[-127, 127]
    const int delta =
        128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
    const uint8 alpha = delta > 0 ? 0 : -delta;
    const uint8 value = delta > 0 ? delta : 0;
    // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度
    cells.push_back(value);
    cells.push_back((value || alpha) ? alpha : 1);
  }   

  // 保存地图栅格数据时进行压缩
  common::FastGzipString(cells, texture->mutable_cells());
  
  // 填充地图描述信息
  texture->set_width(cell_limits.num_x_cells);
  texture->set_height(cell_limits.num_y_cells);
  const double resolution = limits().resolution();
  texture->set_resolution(resolution);
  const double max_x = limits().max().x() - resolution * offset.y();
  const double max_y = limits().max().y() - resolution * offset.x();
  *texture->mutable_slice_pose() = transform::ToProto(
      local_pose.inverse() *
      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

  return true;
}

三、ProbabilityToLogOddsInteger()

现在呢，我们回过头来看一下 ProbabilityGrid::DrawToSubmapTexture() 中调用的 ProbabilityToLogOddsInteger() 函数，该函数实现于 src/cartographer/cartographer/mapping/submaps.h 文件中，其主要原理可以阅读 Cartographer 论文：Real-Time Loop Closure in 2D LIDAR SLAM

// Converts the given probability to log odds.
// 对论文里的 odds(p)函数 又取了 log
inline float Logit(float probability) {
    
    
  return std::log(probability / (1.f - probability));
}

const float kMaxLogOdds = Logit(kMaxProbability);
const float kMinLogOdds = Logit(kMinProbability);

// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
// kMaxLogOdds] is mapped to [1, 255].
inline uint8 ProbabilityToLogOddsInteger(const float probability) {
    
    
  const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) *
                                       254.f / (kMaxLogOdds - kMinLogOdds)) +
                    1;
  CHECK_LE(1, value);
  CHECK_GE(255, value);
  return value;
}

首先来看 Logit 这个函数，这里设代码 $probability/(1.f-probability)=odds$，这里的 odds 与论文中一致，
其输入probability 表示 cell被占用的概率，odds 表示被占用概率与未被占用概率的比值，可想而知：odds 越大，表示cell被占用的概率越大。如果cell被占用的概率与未被占用的概率都未0.5相等时，odds 为1。如果cell未被占用的机率很大很大，那么 odds 是趋向于0的一个数。

也就是说，当占用机率大于未被占用机率时，odds>1；占用小于未被占用机率时，odds<1；如果占用机率等于未被占用机率都为0.5时 odds=1；在根据 log函数(e为底) 的性质，可以把 odds 映射到 $[-\infty ,+\infty ]$，为了方便讲解，映射之后的结果记为 odds’。

很显然，odds’ 直接使用是不合适的，因为其区间为 $[-\infty ,+\infty ]$，论文中使用了一个巧妙的办法，那就 probability 最大值为0.9，那么未被占用的概率最小为0.1，现在回过头来看 src/cartographer/cartographer/mapping/probability_values.h 文件中定义的如下变量：

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

就比较好理解了，ProbabilityToLogOddsInteger()函数由于输入 probability 只能在 [01,0.9] 之间取值，所以可以把 odds’ 夹紧到区间 [kMinLogOdds,kMaxLogOdds]。最后再把区间映射到 [1,255] 区间上。这样就达到了 ProbabilityToLogOddsInteger() 函数的最终目的。
 

四、结语

现在再来回顾一下文章开头的两个疑问：

$疑问1$→ 为什么Grid2D::FinishUpdate()函数中，栅格值为什么需要减去 kUpdateMarker？
$疑问2$→ known_cells_box_ 是什么时候更新，哪里会发生变化，其作用是什么？

对于疑问1，貌似依旧没有获得答案，但是对于疑问二，known_cells_box_ 的作用是记录一块区域，该区域把探测过，更新过的 cell 都包揽进去了，再对 cell 更新的同时需要对 known_cells_box_ 也进行更新。

但是还出现了一个新的疑问：

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

针对于这两个疑问，没有关系，看下在接下来的代码中是否能够找到答案。