(02)Cartographer源码无死角解析-(43) 2D栅格地图→ValueConversionTables、Grid2D

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过前面的博客，已经对 ActiveSubmaps2D、Submap2D、Submap、MapLimits 进行了讲解，但是呢，对于栅格地图的整体框架与轮廓或许依旧十分困惑，来回顾一下：
(02)Cartographer源码无死角解析-(03) 新数据运行与地图保存、加载地图启动仅定位模式
这篇博客钟保存的地图：
在这里插入图片描述
该栅格地图可以理解为一个灰度图，颜色约白的像素，说明该像素对应地图坐标系中 cell 被占用的概率越少，这里的占用可以理解为障碍物。也就是说，越黑的区域其是障碍物的概率越大，如上图中墙壁是黑色的，等价于障碍物。另外，在地图的外面，也就是上图中的灰色区域，表示没有探测的区域，其对应的 cell 区域有可能没有被占用，也可能被占用了。

虽然最终的目的是需要形成这样一副栅格地图，但是源码中又是如何实现的呢？其核心就是 src/cartographer/cartographer/mapping/2d/grid_2d.h 文件中类 Grid2D 的成员变量：

std::vector<uint16> correspondence_cost_cells_;    //地图栅格值

该变量记录着一个 Grid 中所有 cell 对应地图的栅格值(被占用概率越大，该值越大)，根据该成员变量，就可以绘画出一块 Grid 区域的栅格地图。这里需要注意的一个点，correspondence_cost_cells_ 存储的元素为 uint16 类型，那么说明其取值范围是 $0,2^{16}]=[0,65536]$ 。当然，最大值虽然是 65536，但是不一定所有数值都使用到了。

回到 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的 ActiveSubmaps2D::AddSubmap() 函数，可见如下代码：

  // 新建一个子图, 并保存指向新子图的智能指针
  submaps_.push_back(absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));

可知创建一个子图，需要三个参数，第一个参数 origin 表示激光雷达原点在 local 坐标系下的位置，第二个参数为 ProbabilityGrid 类型智能指针对象，第三个参数为 ValueConversionTables 类对象。下面就来了解一下 ValueConversionTables 这个类。

二、ValueConversionTables

1、初始化

首先 class ActiveSubmaps2D中存在成员变量：ValueConversionTables conversion_tables_; 不过在 ActiveSubmaps2D 构造函数中，并没有对该成员进行初始化，但是每次调用 ActiveSubmaps2D::AddSubmap() 函数创建子图 Submap2D 实例时，都会把该参数作为实参传递给 Submap2D 的构造函数，并且时以引用的形式传递，也就是说，无论创建多少个 Submap2D 实例，这些实例都是用 ActiveSubmaps2D 类中的 ValueConversionTables 实例 conversion_tables_。

ValueConversionTables 声明于 src/cartographer/cartographer/mapping/value_conversion_tables.h 之中，只存在一个成员变量如下：

  std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */,
                            float /* upper_bound */>,
           std::unique_ptr<const std::vector<float>>>
      bounds_to_lookup_table_;

该 map 对象的 key 是一个元组，元素包含三个 float 元素，value 是一个装载 float 对象的 vector 容器指针。其有什么作用呢？那么得来看看其唯一得成员函数 GetConversionTable()。

2、GetConversionTable()

该函数 ValueConversionTables::GetConversionTable() 实现于 src/cartographer/cartographer/mapping/value_conversion_tables.cc 中，其需要传递三个参数：

float unknown_result,   //未知时的结果
float lower_bound,  //最小边界值
float lower_bound,  //最大边界值

这三个形参会构建一个 tuple 对象 bounds，然后以该对象作为 key 在成员变量 map 对象 bounds_to_lookup_table_ 中查找，如果没有找到，则会调用 PrecomputeValueToBoundedFloat() 函数创建一个转换表，转换表是一个 std::unique_ptr<std::vector<float>> 对象，其看作 value，然后会把 bounds 做为 key，插入到 map 对象 bounds_to_lookup_table_，并且返回该转换表。

如果bounds_to_lookup_table_ 中已经包含了由三个形参构建的转换表，那么直接返回该转换表。该函数的注释如下：

/**
 * @brief 获取转换表, 这个函数只会调用1次
 * 
 * @param[in] unknown_result 0.9 未知时的值
 * @param[in] lower_bound 0.1 最小correspondence_cost
 * @param[in] upper_bound 0.9 最大correspondence_cost
 * @return const std::vector<float>* 
 */
const std::vector<float>* ValueConversionTables::GetConversionTable(
    float unknown_result, float lower_bound, float upper_bound) {
    
    
  // 将bounds作为key
  std::tuple<float, float, float> bounds =
      std::make_tuple(unknown_result, lower_bound, upper_bound);
  auto lookup_table_iterator = bounds_to_lookup_table_.find(bounds);

  // 如果没有bounds这个key就新建
  if (lookup_table_iterator == bounds_to_lookup_table_.end()) {
    
    
    // 新建转换表
    auto insertion_result = bounds_to_lookup_table_.emplace(
        bounds, PrecomputeValueToBoundedFloat(0, unknown_result, lower_bound,
                                              upper_bound));
    return insertion_result.first->second.get();
  } 
  // 如果存在就直接返回原始指针
  else {
    
    
    return lookup_table_iterator->second.get();
  }
}

总的来说，ValueConversionTables::GetConversionTable() 目的就是根据三个形参获取转换表，如果该转换表不存在，则调用 PrecomputeValueToBoundedFloat() 函数，根据参数构建转换表，然后添加至 bounds_to_lookup_table_ 中，这样下次就可以获取该转换表了。

3、PrecomputeValueToBoundedFloat()

该函数同样实现于 src/cartographer/cartographer/mapping/value_conversion_tables.cc 文件中，其需要四个形数分别为：

const uint16 unknown_value, //默认为0
const float unknown_result, //默认为0.9
const float lower_bound,  //默认为0.1
const float upper_bound //默认为0.9

该函数的目的是比较简单的，其返回一个 std::vector<float> * 型的智能指针对象 result， result 存储元素个数为 uint16 类型的最大值 +1。最终就是把 $[upper\_bound,lower\_bound \backsim upper\_bound ]$ = $0.1\backsim0.9]$ 映射到 [0,32767]。这样后续通过 $[0, 32767]$ 中任意一个值作为索引，则可从 result 中获得 $[0.1, 0.9]$ 之间与之对应的小数值,代码如下所示：

/**
 * @brief 新建转换表
 * 
 * @param[in] unknown_value 0 
 * @param[in] unknown_result 0.9 
 * @param[in] lower_bound 0.1 
 * @param[in] upper_bound 0.9 
 * @return std::unique_ptr<std::vector<float>> 转换表的指针
 */
std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat(
    const uint16 unknown_value, const float unknown_result,
    const float lower_bound, const float upper_bound) {
    
    
  auto result = absl::make_unique<std::vector<float>>();
  // num_values = 65536
  size_t num_values = std::numeric_limits<uint16>::max() + 1; 
  // 申请空间
  result->reserve(num_values);

  // 将[0, 1~32767]映射成[0.9, 0.1~0.9]
  // vector的个数为65536, 所以存的是2遍[0-32767]的映射
  for (size_t value = 0; value != num_values; ++value) {
    
    
    result->push_back(SlowValueToBoundedFloat(
        static_cast<uint16>(value) & ~kUpdateMarker, // 取右边15位的数据, 0-32767
        unknown_value,
        unknown_result, lower_bound, upper_bound));
  }
  return result;
}

这里提及一个点，那就是 num_values 为 65536，所以以存的是2遍 $[0\backsim32767]$ 的映射。

4、SlowValueToBoundedFloat()

映射的核心函数为 SlowValueToBoundedFloat(), 传给该函数的实参如下所示：

SlowValueToBoundedFloat(
        static_cast<uint16>(value) & ~kUpdateMarker, // 取右边15位的数据, 0-32767
        unknown_value,
        unknown_result, lower_bound, upper_bound)

由于 kUpdateMarker = 1u << 15; 所以只取了 value 的右边的15位，第一位没有取，也就是实际传的实参 value 为 $[0, 32767]$ ，源码如下：

/**
 * @brief 将[0, 1~32767] 映射到 [0.9, 0.1~0.9]
 * 
 * @param[in] value [0, 32767]的值, 0 对应0.9
 * @param[in] unknown_value 0
 * @param[in] unknown_result 0.9 
 * @param[in] lower_bound 0.1 下界
 * @param[in] upper_bound 0.9 上界
 * @return float 转换后的数值
 */
float SlowValueToBoundedFloat(const uint16 value, 
                              const uint16 unknown_value,
                              const float unknown_result,
                              const float lower_bound,
                              const float upper_bound) {
    
    
  CHECK_LE(value, 32767); //如果value不小于等于32767，则报错
  //当传入的value == unknown_value时，value 映射的值就是 unknown_result
  if (value == unknown_value) return unknown_result;
  //把 upper_bound - lower_bound 划分成 32766 份，kScale 表示每一份的大小
  const float kScale = (upper_bound - lower_bound) / 32766.f;
  // 把 value 看作份数，其乘 kScale 然后再加上起始值 lower_bound
  // value 为1时，其映射的结果应该为 lower_bound，所以还需要减去 kScale
  return value * kScale + (lower_bound - kScale);
}

从该函数可以看出，当被映射的值 value=unknown_value=0时，其会被映射成 unknown_result=0.9。随后会把 $[1, 32767]$ 映射到 $[0.1, 0.9]$ 。

5、总结

来说呢，ValueConversionTables 中实现了成员函数 GetConversionTable()，该函数会根据传入的三个实参 unknown_result=0.9，lower_bound=0.1，upper_bound=0.9 构建转换表，该转换表可以把 $[0,~1\backsim 32767]$ 映射到 $[0.9,~0.1\backsim 0.9]$ 。

或许有的朋友存在疑问，为什么要构建一个这样的表格，直接计算不好吗？如果每次都根据 SlowValueToBoundedFloat() 函数进行计算，还是比较耗时的，所以这里预先计算出来，用空间换时间，后续通过索引的方式，就可以把 $[0,~1\backsim 32767]$ 转换成 $[0.9,~0.1\backsim 0.9]$ 。

三、Grid2D

通过前面的介绍，了解到所有 Submap2D 对象共用 ActiveSubmaps2D 中转换表 ValueConversionTables conversion_tables_。该转换表的功能是通过索引的方式把 $[0,~1\backsim 32767]$ 数值转换成 $[0.9,~0.1\backsim 0.9]$ 的数值。该转换表再创建子图 Submap2D 时，会传递给 ProbabilityGrid。关于 ProbabilityGrid 的内容后续再进行讲解，ProbabilityGrid 构造函数中又会把转换表传递给 Grid2D。Grid2D 声明于 src/cartographer/cartographer/mapping/2d/grid_2d.h 文件中。首先来看一下其中的成员变量：

 private:
  MapLimits limits_;  // 地图大小边界, 包括x和y最大值, 分辨率, x和y方向栅格数

  // 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知
  std::vector<uint16> correspondence_cost_cells_; 
  float min_correspondence_cost_;  //correspondence_cost_cells_ 中的最小值
  float max_correspondence_cost_;   //correspondence_cost_cells_ 中的最大值
  std::vector<int> update_indices_;               // 记录已经更新过的索引

  // Bounding box of known cells to efficiently compute cropping limits.
  Eigen::AlignedBox2i known_cells_box_;           // 栅格的bounding box, 存的是像素坐标
  // 将[0, 1~32767] 映射到 [0.9, 0.1~0.9] 的转换表
  const std::vector<float>* value_to_correspondence_cost_table_;

其 correspondence_cost_cells_ 是最总要的一个变量，其记录着 Grid2D 中所有 cell 对应的栅格值。

1、Grid2D::Grid2D()

先来看看其构造函数，实现于 grid_2d.cc 文件中：

/**
 * @brief 构造函数
 * 
 * @param[in] limits 地图坐标信息
 * @param[in] min_correspondence_cost 最小correspondence_cost 0.1
 * @param[in] max_correspondence_cost 最大correspondence_cost 0.9
 * @param[in] conversion_tables 传入的转换表指针
 */
Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
               float max_correspondence_cost,
               ValueConversionTables* conversion_tables)
    : limits_(limits), //记录该子图Grid的限制信息

      //对成员变量 correspondence_cost_cells_ 进行初始化，共 num_x_cells * num_y_cells
      //个元素，且初始值都为 0，该值越大，则表示其对应的 cell 被占用机率越大。
      correspondence_cost_cells_(
          limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
          kUnknownCorrespondenceValue),  // 0
      //correspondence_cost_cells_ 中的最大值
      min_correspondence_cost_(min_correspondence_cost),  // 0.1
      //correspondence_cost_cells_ 中的最小值
      max_correspondence_cost_(max_correspondence_cost),  // 0.9
      // 新建转换表
      value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
          max_correspondence_cost, min_correspondence_cost,
          max_correspondence_cost)) {
    
    
  CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
}

现在来看该构造函数就比较简单了，如上所示，主要是对 correspondence_cost_cells_ 进行了初始化，设置默认值为 kUnknownCorrespondenceValue=0。

2、Grid2D::FinishUpdate()

暂时呢，先不理会该函数是在哪里被调用的，或者说什么时候会调用该函数，来看看其功能：

// Finishes the update sequence.
// 插入雷达数据结束
void Grid2D::FinishUpdate() {
    
    
  while (!update_indices_.empty()) {
    
    
    //update_indices_ 记录着已经更新过的cell的索引，获得
    //最近更新的cell栅格值，该值需要大于等于32768
    DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],
              kUpdateMarker);
    // 更新的时候加上了kUpdateMarker, 在这里减去
    correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
    //移除最后一个索引
    update_indices_.pop_back();
  }
}

由代码可知，该函数主要作用就是需要保证 update_indices_ 最后一个元素作为索引，其对应的 cell 栅格值需要大于 kUpdateMarker，然后该栅格值减去 kUpdateMarker。这里暂时留下一个疑惑，那就是这样的做的目的与作用是什么？→ $\color{red} 疑问1$

3、Grid2D::ComputeCroppedLimits()

从命名看该函数，其是从 Limits 计算出一个剪切区域。其传入两个参数：

Eigen::Array2i* const offset,
CellLimits* const limits

const 修饰的是指针 offset 与 limits，也就是说 offset 与 limits 本身存储的地址是不能改变的，但是其指向的目标是可以改变的。这里的所谓的剪切，实际上就是对 offset 与 limits 重新进行赋值。

其赋值的依据来自于成员变量 Eigen::AlignedBox2i known_cells_box_; 该成员变量存储的是多个 cell 对应的像素坐标，实际上呢 offset 记录 known_cells_box_ 中 x,y 的最小值，limits 记录 x,y 最大值与最小值的差值。代码如下所示：

// Fills in 'offset' and 'limits' to define a subregion of that contains all
// known cells.
// 根据known_cells_box_更新limits
void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, //两个元素的数组
                                  //记录着当前Grid x轴y轴上的cell数
                                  CellLimits* const limits) const {
    
     
  //判断一下已知(探索过)的 cell 是否为空
  if (known_cells_box_.isEmpty()) {
    
    
    *offset = Eigen::Array2i::Zero(); 
    *limits = CellLimits(1, 1);
    return;
  }
  *offset = known_cells_box_.min().array();
  *limits = CellLimits(known_cells_box_.sizes().x() + 1,
                       known_cells_box_.sizes().y() + 1);
}

简单的说，基于像素坐标系，offset 表示一个 box 左上角坐标，limits 记录该 box 的宽高加1。从这里可以看出，关于 limits 与 offset 的更新主要依赖于known_cells_box_，那么 known_cells_box_ 又是如何变化更新的呢？暂时先记着→ $\color{red} 疑问2$

4、Grid2D::GrowLimits()

该函数存在两个重载，如下所示(实际上第一个重载函数最终调用了第二个重载函数):

void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
    
    
  GrowLimits(point, {
    
    mutable_correspondence_cost_cells()},
             {
    
    kUnknownCorrespondenceValue});
}

void Grid2D::GrowLimits(const Eigen::Vector2f& point,
                        const std::vector<std::vector<uint16>*>& grids,
                        const std::vector<uint16>& grids_unknown_cell_values) {

从函数的命名可以看到，该函数的目的是为了增大 MapLimits limits_，其主要包含着一个子图(或者说Grid)大小边界，如 x和y最大值, 分辨率, x和y方向栅格数。对地图的扩大，本质上是对这些属性的修改，源码实现的逻辑如下，先来看下图：
在这里插入图片描述
假若传入的地图点(物理坐标) point，其没有被包含在上图的蓝色区域，也就是源码的 limits_ 之中，则会对蓝色地图进行扩大，扩大成绿色区域，从上图可以看出由蓝色地图扩大成绿色地图，其x,y都分别扩大了2倍，那么源码中又是如何实现的呢？

$\color{blue}(1)$ 传入的参数 point 地图的物理坐标，转换成像素坐标之后，判断其对应的 cell 是否位于目前的地图坐标系内，如果不在，则说明现在的地图太小了，需要扩大。

$\color{blue}(2)$ 地图的扩大首先是对 MapLimits limits_ 进行调整，首先地图的原点与分辨率不变，然后地图 x,y 的最大值 max 增加原本最大值的一半，并且把地图 x,y 方向 cell 的数量都扩大至原来的两倍。

$\color{blue}(3)$ 老坐标系的原点在新坐标系下的一维像素坐标 offset，也就是上图中红色点O在紫色点O坐标系的一维坐标。

$\color{blue}(4)$ 根据求得的 offset，将老地图的栅格值拷贝到新地图上，新地图保存保存栅格值的变量为 new_cells，拷贝完成之后执行 *grids[grid_index] = new_cells，用新地图替换掉原来的老地图。

$\color{blue}(5)$ 更新完 limits_ 之中呢，因为地图变了，所以需要对 known_cells_box_ 进行调整，需要把这些坐标回复到老地图中的位置，直接平移 x_offset, y_offset 个单位即可。

$\color{blue}总结$ 其主要思路是先把地图扩大 (x_offset,y_offset) 个单位，然后把原点平移(-x_offset,-y_offset)个单位，然后把旧地图的栅格值复制到新地图上。另外需要注意的是，地图的增加是在 while 循环中，只有 point 位于地图内，才会停止扩充地图。代码注释如下：

// 根据坐标决定是否对地图进行扩大
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
                        const std::vector<std::vector<uint16>*>& grids,
                        const std::vector<uint16>& grids_unknown_cell_values) {
    
    
  CHECK(update_indices_.empty());
  // 判断该点是否在地图坐标系内
  while (!limits_.Contains(limits_.GetCellIndex(point))) {
    
    
    const int x_offset = limits_.cell_limits().num_x_cells / 2;
    const int y_offset = limits_.cell_limits().num_y_cells / 2;
    // 将xy扩大至2倍, 中心点不变, 向四周扩大
    const MapLimits new_limits(
        limits_.resolution(),
        limits_.max() +
            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
        CellLimits(2 * limits_.cell_limits().num_x_cells,
                   2 * limits_.cell_limits().num_y_cells));
    const int stride = new_limits.cell_limits().num_x_cells;
    // 老坐标系的原点在新坐标系下的一维像素坐标
    const int offset = x_offset + stride * y_offset;
    const int new_size = new_limits.cell_limits().num_x_cells *
                         new_limits.cell_limits().num_y_cells;

    // grids.size()为1
    for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
    
    
      std::vector<uint16> new_cells(new_size,
                                    grids_unknown_cell_values[grid_index]);
      // 将老地图的栅格值复制到新地图上
      for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
    
    
        for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
    
    
          new_cells[offset + j + i * stride] =
              (*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
        }
      }
      // 将新地图替换老地图, 拷贝
      *grids[grid_index] = new_cells;
    } // end for
    // 更新地图尺寸
    limits_ = new_limits;
    if (!known_cells_box_.isEmpty()) {
    
    
      // 将known_cells_box_的x与y进行平移到老地图的范围上
      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
    }
  }
}

四、结语

通过上面的分析，对于 Grid2D 类中比较重要的几个函数都分析完成了，这些函数都位于 grid_2d.cc 之中，但是头文件 grid_2d.h 还由一些其他的函数，如下所示：

  // 返回可以修改的栅格地图数组的指针
  std::vector<uint16>* mutable_correspondence_cost_cells() {
    
    
    return &correspondence_cost_cells_;
  }
  std::vector<int>* mutable_update_indices() {
    
     return &update_indices_; }
  Eigen::AlignedBox2i* mutable_known_cells_box() {
    
     return &known_cells_box_; }

从命名上可以知道，以上三个函数的返回值是可以被修改的，也就是说，通过其获取 correspondence_cost_cells_、update_indices_、known_cells_box_ 变量之后，可以对这些变量进行修改，从而达到更新的目的。

另外，该篇博客还存在如下两个疑问：

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

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

这些疑问，在后面的源码中我们都是可以找到答案的，所以各位先不要着急。