cartographer 代码思想解读（5）- probability grid地图更新2

上节讲解了probability类以及基类grid存储内容，同时也分析了cartographer采用的概率初始化和更新的思想。本节接上节，分析cartographer概率更新代码具体实现。

gridmap插入新的range

cartographer将新的一帧激光scan经过前端匹配，获取最优位置然后插入gridmap中，即扩展和更新原来的gridmap，接上节可知gridmap为probability 类型，表明要根据新的一帧激光数据将其扩展至probability_map中。

submap和submap2d两个类含义

其中cartographer每一次插入的地图均为submap（cartographer特色，即将整个地图分为一个个submap，所有submap组合成全局地图）。同样submap为一个基类，由于我们只分析2d slam的情况，因此submap2D类为其继承类。而新的激光scan插入具体实现在submap2D类中。其代码目录为
cartographer/mapping/submap.h
cartographer/mapping/2d/submap2d.h。
其中基类submap中具体内容如下，而其内含方法仅是对参数获取和设置。

  //仅三个变量，submap的位置， 激光帧个数， 插入完成标志位
  const transform::Rigid3d local_pose_;
  int num_range_data_ = 0;
  bool insertion_finished_ = false;

而submap2d继承类增加内容如下，包括grid具体地图和概率与uint16转换表格。

  std::unique_ptr<Grid2D> grid_;    // 存储概率地图
  ValueConversionTables* conversion_tables_;  // 浮点数与到uint16转换表格，估计用于概率图转换成整型进行计算

其中conversion_tables_表示0~32767的整型数对应的概率值，上一节中讲解了其具体相互转换关系，由于其概率上边界和下边界都是预配置的，因此其对应关系为固定，可以提前计算好所有0-32767所有value对应的概率，后续使用时直接查表即可，提高运行速度。cartographer算法中存在类似的优化性能方案，即采用空间换取速度。其查询表格实现代码目录为：
cartographer/mapping/value_conversion_tables.h

// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].
// 具体转换代码，
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);
  if (value == unknown_value) return unknown_result;
  const float kScale = (upper_bound - lower_bound) / 32766.f;
  return value * kScale + (lower_bound - kScale);
}

// 提前计算所有uint16中的对应有边界范围的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>>();
  size_t num_values = std::numeric_limits<uint16>::max() + 1;
  result->reserve(num_values);
  // 生成的表格保存至vector中
  for (size_t value = 0; value != num_values; ++value) {
    result->push_back(SlowValueToBoundedFloat(
        static_cast<uint16>(value) & ~kUpdateMarker, unknown_value,
        unknown_result, lower_bound, upper_bound));
  }
  return result;
}

submap2d 插入器

新的一帧激光scan插入到grid中，其具体接口是在submap2d类中，其代码如下：

//在本submap中插入新的激光数据，参数：1.激光数据； 2. 激光插入器
void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) {
  CHECK(grid_);
  CHECK(!insertion_finished());
  // 激光数据帧插入grid中， 采用虚函数接口调用， 根据不同地图类型具体实现
  range_data_inserter->Insert(range_data, grid_.get());
  // 激光数据帧个数自加1
  set_num_range_data(num_range_data() + 1);
}

从代码可看出其中range_data为新的激光scan，grid为submap2D的全局变量，同时引入了range_data_inserter叫做插入器的类，即具体实现range_data插入到grid中。
其中range_data类型为cartographer算法使用的内部类型，不同于ROS一般定义的msg格式，并非为距离信息，而是根据距离信息和激光位置转换后的点云信息，其格式类型如下，包含激光当前位置origin和激光距离对应的点云信息returns和misses。其中点云信息可由msg格式进行转换，可在cartographer_ros代码中bridge中找到。

// origin 到 misses之间均为 空闲空间
// returns则为 hit空间
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;   // 有效值, 即有效反射距离值
  PointCloud misses;    // 无效值，即测距最远值对应的坐标
};

而range_data_inserter为新引入的插入器，其中Insert 函数这里调用了其接口RangeDataInserterInterface类，内部为虚函数，代码目录为：
cartographer/mapping/range_data_inserter_interface.h
由于我们采用probability地图构成的submap，因此其真正实现在probability的派生类ProbabilityGridRangeDataInserter2D中。代码目录为：
cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h

// submap插入新帧scan 刷新submap
// input : range_data,  grid
// output : 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).
  // 采用画线法更新地图
  CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
           probability_grid);
  probability_grid->FinishUpdate();
}

显然插入器采用了一种CastRays方法具体实现栅格概率更新，本质类似于画直线仅考虑激光光束经过的栅格进行更新。

CastRays实现

// 计算hit 栅格， 同时计算传感器到到hit栅格经过的 miss栅格
// input :insert_free_space配置项，默认为true，表示需要更新miss情况的概率
void CastRays(const sensor::RangeData& range_data,
              const std::vector<uint16>& hit_table,
              const std::vector<uint16>& miss_table,
              const bool insert_free_space, ProbabilityGrid* probability_grid) {
  // 根据 新的range 更新grid的边界大小
  GrowAsNeeded(range_data, probability_grid);
  // 获取边界
  const MapLimits& limits = probability_grid->limits();
  // 将分辨率提高kSubpixelScale
  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
  // 重新定义地图边界
  const MapLimits superscaled_limits(
      superscaled_resolution, limits.max(),
      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                 limits.cell_limits().num_y_cells * kSubpixelScale));
  // 获取激光点云起点坐标
  const Eigen::Array2i begin =
      superscaled_limits.GetCellIndex(range_data.origin.head<2>());
  // Compute and add the end points.
  // 获取激光端点，即有效反射点，同时为hit点
  std::vector<Eigen::Array2i> ends;
  ends.reserve(range_data.returns.size());
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
    ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
    // 针对每个hit端点进行更新栅格概率，通过hit_table表格查询,如当前为p 则，新的p = hit_table[p]
    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
  }

  // 若无需更新miss栅格单元，可直接退出
  if (!insert_free_space) {
    return;
  }

  // Now add the misses.
  // origin 到 hit之间均为miss
  for (const Eigen::Array2i& end : ends) {
    // breshman 画线法获取两点间的数据，最后一个参数用于还原原分辨率
    std::vector<Eigen::Array2i> ray =
        RayToPixelMask(begin, end, kSubpixelScale);
    // 对所有点进行miss 更新
    for (const Eigen::Array2i& cell_index : ray) {
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }

  // Finally, compute and add empty rays based on misses in the range data.
  // 更新所有 range中miss的点， 则整条光速直线均为miss更新
  for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
    std::vector<Eigen::Array2i> ray = RayToPixelMask(
        begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
        kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }
}

简单总结：CastRays中接口包括新scan数据，miss_table和miss_table表格，以及需要插入的概率地图。其中miss_table和hit_table是用于更新grid概率值的查询表格，即已知当前value，通过查表可获知更新后的value，其表格生成过程下面会单独讲解。

执行步骤：
1.GrowAsNeeded实现当前grid map边界的扩展，即由于新的scan加入，可能会导致地图变大；
2.将地图分辨提高kSubpixelScale=1000倍，目的是为后面画直线精度更加精确；
3.获取高分辨率地图下的激光原点range_origin坐标索引;
4.获取高分率地图下所有有效激光点云的坐标索引；
5.获取还原原始地图分辨率坐标cell的value，然后查询hit_table表格进行更新，即z=hit条件下的更新；
6.采用RayToPixelMask画线的方法，获取激光原点到点云之间直线的所有点坐标；
7.通过还原原始地图分辨率获取value，查询miss_table表格进行更新，即z=miss条件下的更新；

hit_table和miss_table更新查询表

上节已经详细分析了probability grid cartographer算法使用的更新理论和过程，即value_new=C(z) * value。从公式上看十分简单，仅有一个乘法运算，且系数为提前配置的两个常数。cartographer为了进一步提高效率，提前计算了所有value（1-32767）空间更新后的结果并放入了hit_table和miss_table中，即牺牲空间换效率的方法，cartographer常用方法。故后续采用更新时，仅需查表可获得更新后的结果，而无需临时进行乘法运算，从而节约大量时间。
调用代码目录如下，即在插入器类构造函数调用：

// 构造函数，计算出hit和miss占用栅格率更新表格
// 栅格更新可认为是，当前栅格的hit和miss概率值， 经过新的观测（即是否miss还是hit），进行更新
// 表格可认为是查表，即加快更新速度
// 注意：由于真正栅格内存储的数据均转换为0~32767的整数，可认为是概率对应值。同理概率更新后也同样为整数
// 表格查询方式： 当前概率为p， 若观测为hit，则新的p = hit_table_[p]
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
    const proto::ProbabilityGridRangeDataInserterOptions2D& options)
    : options_(options),
      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.hit_probability()))),                         //转换为odd表示
      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.miss_probability()))) {}

代码目录如下：
cartographer/map/probability_values.h
代码如下：

std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
    float odds) {
  std::vector<uint16> result;
  result.reserve(kValueCount);
  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
                       ProbabilityFromOdds(odds))) +
                   kUpdateMarker);
     // 针对每一个value均乘以更新系数odds
  for (int cell = 1; cell != kValueCount; ++cell) {
    result.push_back(
        CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                odds * Odds(CorrespondenceCostToProbability(
                           (*kValueToCorrespondenceCost)[cell]))))) +
        kUpdateMarker);
  }
  return result;
}

由于上节已经讲过hit概率，free概率，odd，整数value之间转换关系，因此上段代码较为简单。即将更新过程提前计算，即所有value均乘以更新系数（C_hit和C_miss）并放入表格中。

**思考:
1.这里有个疑问就是每个更新的value均加上了kUpdateMarker这个常数，而在画线后执行了 probability_grid->FinishUpdate()，即将已更新的所有坐标value均减去kUpdateMarker这个常数；
2.在表示栅格概率值采用了odd，但是其他slam中将其进一步表示log（odd），如此更新函数value_new=C(z) * value，可变成log（value_new） = log（C(z)）+ log(value)，即乘法变成加法，提高效率。在submaps.h文件中有此转换关系，但暂时未找到在哪应用。
**