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());// 激光数据帧个数自加1set_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();// 将分辨率提高kSubpixelScaleconst 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之间均为missfor (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均乘以更新系数oddsfor (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文件中有此转换关系，但暂时未找到在哪应用。
**