占用栅格的数据结构

在上文中，我们研究了Cartographer中用于描述二维子图的类Submap2D。了解到该类只是一个封装， 子图的详细信息，比如概率栅格的占用情况，子图的大小等都是在一个叫grid_的对象中记录的。虽然对象grid_的数据类型是Grid2D， 但ActiveSubmaps2D在函数CreateGrid中实际构建的是ProbabilityGrid类型。 子图也是一种占用栅格形式的地图，本文从ProbabilityGrid开始分析占用栅格的数据结构和接口。

1. ProbabilityGrid

ProbabilityGrid是从Grid2D派生而来的，在它的定义中没有额外的添加成员变量，它只是通过一些函数来解释栅格单元的占用概率。它提供了两个构造函数，这里我们只关心类ActiveSubmaps2D所使用的构建方式， 下面是对应的构造函数。该函数有一个MapLimits类型的参数， 它用于描述地图的边界，我们将在本文后面详细分析它。在初始构造列表中，可以看到参数limits和另外两个常数被用来构造父类Grid2D。

        ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost) {}

用于构造父类的两个常数分别描述了栅格元素的最小值和最大值， 它们被定义在文件probability_values.h中。 如下面的代码片段所示，可以看到各个栅格的最小值是0.1，最大值是0.9。

        constexpr float kMinProbability = 0.1f;
        constexpr float kMaxProbability = 1.f - kMinProbability;
        constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability;
        constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;

ProbabilityGrid通过函数SetProbability赋予单元索引cell_index概率值probability，在调用该函数之前需要保证对应的栅格单元的概率是未知的。 在第2行中根据索引cell_index获取目标栅格单元，这里调用的两个函数都是在父类中定义的，将在介绍Grid2D的时候详细拆解。然后在第3行中检查栅格单元是否未知。 常数kUnknownProbabilityValue也是在文件probability_values.h中定义的， 其值为0。接着在第4行中为栅格单元赋予新值，最后通过父类Grid2D的接口记录下栅格单元索引。

        void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, const float probability) {
            uint16& cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
            CHECK_EQ(cell, kUnknownProbabilityValue);
            cell = CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
            mutable_known_cells_box()->extend(cell_index.matrix());
        }

函数SetProbability在为栅格单元赋予新值的时候，调用了两个在文件probability_values.h中定义的函数。 其中函数CorrespondenceCostToValue是将float类型的数据转换为uint16类型，并将输入从区间\([\text{kMinCorrespondenceCost}, \text{kMaxCorrespondenceCost}]\)映射到\([1, 32767]\)。 函数ProbabilityToCorrespondenceCost实际上是对输入概率值取反，这意味着如果我们的输入描述的是栅格单元的占用概率，那么实际存储的则是栅格单元空闲的概率。

函数GetProbability则是用来获取栅格单元的占用概率的。首先在第2行中通过父类的接口判定一下输入的单元索引是否在子图的范围内，如果不在就直接返回一个最小的概率。 接下来就是获取栅格对应的占用概率并返回。其中函数CorrespondenceCostToProbability和ValueToCorrespondenceCost所完成的工作就是刚刚介绍的两个函数的逆。

        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)]));
        }

按照Correspondence Cost与Probability之间的映射关系，应该说Probability是指栅格被占用的概率，而Correspondence Cost则是栅格空闲的概率。为了书写方便，以后我们用\(p_{occ}\)表示Probability， \(p_{free} = 1.0 - p_{occ}\)表示Correspondence Cost。

一开始没太弄明白函数ApplyLookupTable是干嘛使得，后来看了ActiveSubmaps2D所用的ProbabilityGridRangeDataInserter2D类型的插入器对象后了解到，这个函数是用来通过查表来更新栅格单元的占用概率的。 这也就是为什么Cartographer会费那么多精力把浮点数转换为uint16。下面是该函数的代码片段，该函数有两个输入参数，其中cell_index是将要更新的栅格单元索引，table是更新过程中将要查的表。

        bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector<uint16>& table) {
            // 在函数的一开始的时候，先检查一下查找表的大小。
            DCHECK_EQ(table.size(), kUpdateMarker);
            // 然后通过cell_index计算栅格单元的存储索引，获取对应的\(p_{free}\)存储值，并确保该值不会超出查找表的数组边界。
            const int flat_index = ToFlatIndex(cell_index);
            uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
            if (*cell >= kUpdateMarker)
                return false;
            // 接着通过父类的接口记录下当前更新的栅格单元的存储索引flat_index。

            mutable_update_indices()->push_back(flat_index);
            // 通过查表更新栅格单元
            *cell = table[*cell];
            DCHECK_GE(*cell, kUpdateMarker);
            // 最后在通过父类标记cell_index所对应的栅格的占用概率已知
            mutable_known_cells_box()->extend(cell_index.matrix());
            return true;
        }

函数ComputeCroppedGrid用于裁剪子图。因为在更新子图的过程中，并不能保证更新的数据能够完整覆盖整个子图的所有栅格。该函数就是以最小的矩形框出已经更新的栅格，下面是该函数的代码片段。 在该函数的一开始，就先通过父类的接口获取包含所有已知栅格的子区域，变量offset记录了子区域的起点，cell_limits描述了从offset开始的矩形框。(此处应该有图)

        std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
            Eigen::Array2i offset;
            CellLimits cell_limits;
            ComputeCroppedLimits(&offset, &cell_limits);
            // 接着获取子图的分辨率和最大的xy索引，并构建一个新的ProbabilityGrid对象cropped_grid。
            const double resolution = limits().resolution();
            const Eigen::Vector2d max = limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
            std::unique_ptr<ProbabilityGrid> cropped_grid = common::make_unique<ProbabilityGrid<(MapLimits(resolution, max, cell_limits));
            // 最后在遍历了对象cropped_grid中的所有栅格，拷贝占用概率之后，返回对象退出。
            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(cropped_grid.release());
        }

此外ProbabilityGrid还重载了两个虚函数ToProto和DrawToSubmapTexture，它们主要用于对象的序列化，不是重点不管它。

2. Grid2D

下面我们来分析ProbabilityGrid的父类Grid2D，该类才提供了实际用于存储占用栅格的数据结构。 它是由接口GridInterface派生出来的类， 但它只是个空壳子，没有定义任何成员函数或者变量。下表列出了Grid2D中定义的所有成员变量。

Grid2D中定义的各个成员函数大都是用来访问它的成员变量的，只有FinishUpdate和GrowLimits两个函数还有一些业务，分别用于停止更新和扩展子图范围。没有什么特殊的操作需要解释。

3. MapLimits

类型MapLimits用于描述地图的范围，它有三个成员变量，如下表所示。

其中对象cell_limits_的数据类型定义在文件xy_index.h中， 它是一个结构体，只有两个成员变量：

4. 完

本文，我们主要是在分析类ProbabilityGrid的成员变量和成员函数，它对外提供了设置、获取以及更新栅格单元的占用概率的接口。它的父类Grid2D则定义了栅格单元的存储方式， 并通过类MapLimits来描述占用地图的作用范围。

通过分析，我们还发现虽然ProbabilityGrid的接口都是占用概率，但Grid2D中的存储都是空闲概率。而且Cartographer通过查表的方式来更新栅格单元的占用概率，Grid2D使用uint16的形式存储数据。