查找表与占用栅格更新
在前两篇文中，我们分析了子图的维护和封装以及占用栅格的数据结构。 无论是封装子图的类Submap2D还是描述占用栅格的Grid2D都没有直接提供把激光雷达的扫描数据插入子图的方法，而是通过ActiveSubmaps2D所提供的插入器对象range_data_inserter_完成的。 这是一种类似于配件的设计模式，如果我们有很多种插入方法，可以分别以插入器的形式实现它们，在ActiveSubmaps2D中根据需要具例化对象，如此我们就能够随意的切换算法， 不需要对其他模块作出过多的改动。
此外在占用栅格的数据结构一文中，我们还了解到Cartographer通过查表的方式完成占用栅格的更新操作， 但并没有分析具体的实现方式。本文中，我们将解析类ProbabilityGridRangeDataInserter2D，讨论激光扫描的RayCasting模型，分析栅格单元的占用概率更新方法。
1. 插入器
通过对类型ActiveSubmaps2D的分析， 看到对象range_data_inserter_的数据类型是ProbabilityGridRangeDataInserter2D。下面的代码片段是该类的定义，它屏蔽了拷贝构造和拷贝赋值。通过函数Insert将激光雷达的数据插到栅格中。它有三个成员变量， 其中options_记录了插入器的配置，hit_table_与miss_table_是用于更新栅格单元的占用概率的查找表。
class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface {
public:
explicit ProbabilityGridRangeDataInserter2D(const proto::ProbabilityGridRangeDataInserterOptions2D& options);
ProbabilityGridRangeDataInserter2D(const ProbabilityGridRangeDataInserter2D&) = delete;
ProbabilityGridRangeDataInserter2D& operator=(const ProbabilityGridRangeDataInserter2D&) = delete;
// Inserts 'range_data' into 'probability_grid'.
virtual void Insert(const sensor::RangeData& range_data, GridInterface* grid) const override;
private:
const proto::ProbabilityGridRangeDataInserterOptions2D options_;
const std::vector<uint16> hit_table_;
const std::vector<uint16> miss_table_;
};
下面是该类型的构造函数， 它通过定义在probability_values.h文件中的函数ComputeLookupTableToApplyCorrespondenceCostOdds完成查找表hit_table_和miss_table_的初始化工作。
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(const proto::ProbabilityGridRangeDataInserterOptions2D& options)
: options_(options),
hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(options.hit_probability()))),
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(options.miss_probability())))
{}
下面是函数Insert的实现。它有两个参数，其中range_data是将要插入的扫描数据，grid则是栅格地图。在函数的一开始将grid强制转换为ProbabilityGrid类型的数据， 所以这个插入器只适配ProbabilityGrid类型的栅格地图。 接着通过文件ray_casting.cc中实现的函数CastRays完成RayCasting操作， 并更新栅格地图。最后调用栅格地图的接口结束更新。
void ProbabilityGridRangeDataInserter2D::Insert(const sensor::RangeData& range_data, GridInterface* const grid) const {
ProbabilityGrid* const probability_grid = static_cast(grid);
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), CHECK_NOTNULL(probability_grid));
probability_grid->FinishUpdate();
}
2. 查找表
我们先来看一下Cartographer更新栅格单元的占用概率的理论 ，对论文的IV章B节的内容简单解读一下。 用\(s\)表示栅格单元的状态，它只有两种选择，(\(s = 1\))表示被占用，(\(s = 0\))表示空闲，这两种状态的概率分别用\(p(s = 1)\)和\(p(s = 0)\)来表示。
激光雷达的工作原理就是测量周围障碍物到传感器的距离，根据激光扫过时的角度和距离，我们可以在扫描的扇面上把测量到的障碍点打印出来，如右图中的线段所示。 栅格地图是按照一定的分辨率把坐标空间划分称为一个个的方格子，障碍点一定会落在某个方格内。如右图中的灰色带叉的区域，我们说在这些栅格上观测到了一次hit事件，称之为hit栅格。 而从传感器所在的栅格到hit栅格之间所有栅格就应该是空闲的，如右图中的灰色区域，在这些栅格上观测到了一次miss事件，称之为miss栅格。图中白色的区域表示在这次扫描过程中， 对应的扇区方向上没有发现障碍，并不意味着这些栅格就是空闲的，所以称为未知栅格。我把这种从扫描得到的距离信息转换为栅格的hit或者miss事件的过程称为RayCasting。
我们用\(z\)表示一次扫描过程中栅格的观测事件，(\(z = 1\))表示hit事件，(\(z = 0\))表示miss事件。 对于未知栅格我们的观测过程并没有获得过多的有用信息，一般不处理它保持原样。那么栅格的更新过程就是一个两状态之间的估计问题， 求解过程可以参考《PR》一书第四章介绍的二值贝叶斯滤波器。 在Cartographer中使用的是差异比\(odds(p) = \cfrac{p}{1 - p}\)来进行状态估计的。根据贝叶斯公式有:
$$ \begin{equation}\label{f1} \begin{array}{rcl} p(s = 1 | z) & = & \cfrac{p(z | s = 1)p(s = 1)}{p(z)} \\ p(s = 0 | z) & = & \cfrac{p(z | s = 0)p(s = 0)}{p(z)} \\ \end{array} \Rightarrow odds(s|z) = \cfrac{p(z | s = 1)p(s = 1)}{p(z | s = 0)p(s = 0)} = \cfrac{p(z | s = 1)}{p(z | s = 0)}odds(s) \end{equation} $$上式其实是一个迭代的式子。对于已有观测数据的栅格，就可以通过上式根据观测事件\(z\)和上次观测的\(odds_{k-1} = odds(s)\)更新差异比\(odds_k = odds(s|z)\)。 如果栅格单元还处于未知的状态，那么\(odds_0 = 1\)表示栅格单元被占用或者空闲的概率相等。所以式(\(\ref{f1}\))可以写成如下的形式:
$$ \begin{equation}\label{f2} odds_k = C(z) \bullet odds_{k-1} \end{equation} $$其中\(C(z) = \cfrac{p(z | s = 1)}{p(z | s = 0)}\)称为更新系数。由于\(z\)只有两种状态，所以\(C(z)\)也只有两种取值，用记号\(C_{hit}, C_{miss}\)分别表示hit事件和miss事件发生时的更新系数。 这两个系数与传感器的特性有关，需要根据不同的传感器调整。所以在刚才的构造函数中，可以看到从options对象中获取相应的配置。 如果栅格地图以差异比的形式表示占用概率，那么更新过程就只是一个乘法运算，效率也还可以。但Cartographer还是想要进一步的提高效率，它以空间换取时间，构建了hit表和miss表。 如果发生了hit事件，就从hit表中查找更新后的数据。发生了miss事件则查找miss表。
函数ComputeLookupTableToApplyCorrespondenceCostOdds用于构建查找表，在分析该函数之前，我们先来看一下传参。下面的两行代码是我们刚刚在插入器构造函数看到的用于构建hit表和miss表的。 配置项hit_probability和miss_probability是定义在文件probability_grid_range_data_inserter_options_2d.proto中的两个字段，通过函数Odds转换为更新系数\(C_{hit}\)和\(C_{miss}\)。hit_probability的设置不能小于0.5，miss_probability的设置不能大于0.5。函数Odds完全就是对差异比公式的代码描述，不再细述。
hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(options.hit_probability()))),
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(options.miss_probability())))
下面是函数ComputeLookupTableToApplyCorrespondenceCostOdds的代码片段，它的输入参数就是\(C_{hit}\)或者\(C_{miss}\)。在函数一开始先临时建了一个容器， 并对odds求逆将其转换成概率值(ProbabilityFromOdds)，再取反(ProbabilityToCorrespondenceCost)，然后转换成为存储值(CorrespondenceCostToValue)，最后加上kUpdateMarker把结果塞进了result中。 实际上这个费了牛劲计算出来的值对应着未知栅格的更新。
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(float odds) {
std::vector<uint16> result;
result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(odds))) + kUpdateMarker);
然后在一个for循环中以同样费劲方式依次计算存储值从1到32767所对应的更新值并塞进result中返回。与上面的代码不同的是，这里应用了式(\(\ref{f2}\))。
for (int cell = 1; cell != 32768; ++cell) {
result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
odds * Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))))) + kUpdateMarker);
}
return result;
}
看到这里加上了kUpdateMarker我终于理解了为什么Grid2D的成员函数FinishUpdate要写成下面的样子， 在第4行中将存储值都减去一个kUpdateMarker。但是不理解这一加一减的意义在哪里。
void Grid2D::FinishUpdate() {
while (!update_indices_.empty()) {
DCHECK_GE(correspondence_cost_cells_[update_indices_.back()], kUpdateMarker);
correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
update_indices_.pop_back();
}
}
3. RangeData
struct RangeData {
Eigen::Vector3f origin;
PointCloud returns;
PointCloud misses;
};
在分析函数CastRays之前，我们先来研究一下它的一个输入参数类型RangeData。 它是定义在文件range_data.h中的一个结构体，其定义如右侧的代码片段所示。
它有三个字段，其中origin用于描述当次扫描测量时激光雷达的位置，字段returns和misses记录了扫描到的hit点与miss点。所谓的hit点是指在该点上扫描到了障碍物，该点所在的栅格单元就发生了一次hit事件。 miss点所在的位置上并没有检测到障碍物，只是以传感器的最远有效距离记录下坐标而已。origin到hit点和miss点之间的空间都将被认为是空闲的，所对应的栅格单元则被看作发生了一次miss事件。
字段returns和misses的数据类型是定义在point_cloud.h中的三维向量。 对于我们的二维地图而言，\(z\)轴的数据也就是向量的第三个元素为0。
typedef std::vector<Eigen::Vector3f> PointCloud;
实际上，这个RangeData是经过转换之后的数据，并不是雷达原始的扫描数据。雷达的原始数据是一组距离值，它描述的激光扫过的扇面上距离最近的障碍物到传感器的距离。 在我们demo中，这些原始数据是用ROS系统中的消息记录了。 cartographer_ros还使用SensorBridge把原始数据转换成Cartographer系统中的数据结构。
但是当时并没有直接转换为这里的RangeData，而是一个带有时间戳的点云类型TimedPointCloud。该数据类型只是将原始数据中的距离和扫描角度信息转换为空间点的坐标，并没有区分hit点和miss点。 在Local SLAM中，把激光的测量数据插入到子图的操作， 是从调用LocalTrajectoryBuilder2D的成员函数AddRangeData开始的， 下面是该函数的声明，从它的参数列表中可以看到，那个时候接收的数据都还是TimedPointCloud类型的，至于这个函数中发生了什么事情，我们专门有一篇文章来分析。
std::unique_ptr<MatchingResult> AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& range_data);
4. 函数CastRays
函数CastRays一方面根据RangeData类型的扫描数据完成RayCasting操作，获得一次扫描测量过程中相关栅格的观测事件；另一方面，调用占用栅格的接口完成查表更新。 下面是该函数的代码片段，一共有五个输入参数。其中range_data就是将要处理的扫描数据；hit_table和miss_table则是更新栅格单元的占用概率时需要的查找表，分别用于hit事件和miss事件； insert_free_space是一个配置项，指是否更新发生miss事件的栅格单元的占用概率；最后，probability_grid则是将要更新的占用栅格。
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* const probability_grid) {
在该函数的一开始先通过函数GrowAsNeeded适当的调整栅格地图的作用范围，让它能够覆盖雷达的所有扫描数据。
GrowAsNeeded(range_data, probability_grid);
然后获取扩展之后的作用范围，并构建一个分辨率更高的Maplimits对象，将一个栅格单元分割成\(kSubpixelScale \times kSubpixelScale\)个小块，这样是为了提高RayCasting的精度。
const MapLimits& limits = probability_grid->limits();
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));
获取激光射线的起点在精细栅格中的索引，记录在begin对象下。
const Eigen::Array2i begin = superscaled_limits.GetCellIndex(range_data.origin.head<2>());
遍历所有hit点，用容器ends记录下hit点在精细栅格中的索引。在第14行中调用了占用栅格对象的ApplyLookupTable函数查表更新占用概率。这次调用的传参有两个， 第一个参数将精细栅格下hit点索引重新转换成原始栅格分辨率下的索引，第二个参数就是待查的hit表。
std::vector<Eigen::Array2i> ends;
ends.reserve(range_data.returns.size());
for (const Eigen::Vector3f& hit : range_data.returns) {
ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
}
如果配置不需要处理miss事件，直接返回就可以了。
if (!insert_free_space)
return;
然后通过函数CastRay处理射线起点到hit点之间的栅格，把它们都看做是发生了miss事件的栅格，查找miss_table更新占用概率。但是需要注意这里的begin和end都是精细栅格下的索引。
for (const Eigen::Array2i& end : ends)
CastRay(begin, end, miss_table, probability_grid);
最后处理miss点的情况，我们同样认为射线起点到miss点之间的栅格发生的都是miss事件，调用函数CastRay进行RayCasting并通过miss_table更新相应的占用概率。
for (const Eigen::Vector3f& missing_echo : range_data.misses)
CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), miss_table, probability_grid);
}
5. 完
本文中我们分析了插入器对象range_data_inserter_，介绍了栅格单元的占用概率更新原理，并详细分析了查找表的构建方法。