子图的维护与封装

在上文中，我们分析了Local SLAM的核心对象，但是留下了扫描匹配、 子图更新等核心功能的具体实现。在分析这些核心功能之前，让我们先来看一下子图的数据结构。子图是Local SLAM的各个核心功能处理的目标，对于该数据结构的理解，有助于我们后序分析功能的实现。

本文中，我们顺着LocalTrajectoryBuilder2D中维护的子图对象active_submaps_，来分析封装子图的两个类ActiveSubmaps2D和Submap2D。

1. ActiveSubmaps2D

在类LocalTrajectoryBuilder2D中，通过对象active_submaps_来维护子图，它是一个ActiveSubmaps2D类型的数据。除了刚开始构建该对象的时候，只有一个子图(Submap2D)，其他时候它都维护着两个子图对象。 一个子图用于进行扫描匹配，称为旧图。另一个子图被称为新图用于插入扫描数据。当新图中插入一定数量的数据完成了初始化操作之后，它就会被当作旧图，用于扫描匹配。 对象active_submaps_将抛弃原来的旧图，并重新构建一个新图。下表中列出了类LocalTrajectoryBuilder2D中定义的所有成员变量。

让我们再来看一下它的构造函数。该函数有一个参数用于配置子图的选项，在它的成员初始化列表中，可以看到直接将之拿来构建了对象options_。 此外，该函数还通过私有的成员函数CreateRangeDataInserter构建了一个插入器对象，并赋予了range_data_inserter_。然后在函数体中通过函数AddSubmap，构建了第一个子图。

        ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
            : options_(options), range_data_inserter_(std::move(CreateRangeDataInserter())) {
            // We always want to have at least one likelihood field which we can return,
            // and will create it at the origin in absence of a better choice.
            AddSubmap(Eigen::Vector2f::Zero());
        }

接下来先分析函数AddSubmap，再来看函数CreateRangeDataInserter。下面是函数AddSubmap的代码，该函数有一个输入参数origin，应该是指新建子图的原点坐标。 刚才我们在构造函数中因为没有过多的信息，所以只是简单的给了一个\(\begin{bmatrix} 0 & 0 \end{bmatrix}^T\)的初值。

然后该函数就先检查一下容器submaps_中的子图数量，如果不只有一个子图，就需要在函数FinishSubmap中完成新旧图的切换工作。在构造函数中我们是第一次调用该函数， 会直接构建一个新的Submap2D类型的对象放置到容器submaps_中，它将作为一个新图用于插入数据。在构建Submap2D类型的对象的时候，还调用了函数CreateGrid函数为该对象提供了一个保存栅格占用信息的存储结构。

        void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
            if (submaps_.size() > 1)
                FinishSubmap();

            submaps_.push_back(common::make_unique<Submap2D>(origin, std::unique_ptr<Grid2D>(static_cast<Grid2D*>(CreateGrid(origin).release()))));
            LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
        }

下面是函数CreateRangeDataInserter的实现，很简单只有一句话，就是将实例化的插入器返回而已。 但是需要关注一下该函数的返回值和用于保存插入器对象的变量的数据类型都是接口类RangeDataInserterInterface， 而真正的插入器对象的数据类型是ProbabilityGridRangeDataInserter2D。 从命名上就可以看出，这应该是一种概率栅格形式的地图。

        std::unique_ptr<RangeDataInserterInterface> ActiveSubmaps2D::CreateRangeDataInserter() {
            return common::make_unique<ProbabilityGridRangeDataInserter2D>(options_.range_data_inserter_options().probability_grid_range_data_inserter_options_2d());
        }

函数FinishSubmap的主要任务就是完成新旧图的切换。如下面的代码所示，该函数首先在2,3行中获取当前的旧图，通过旧图对象的成员函数Finish完成一些收尾工作。 然后增加matching_submap_index_记录当前新图索引，最后抛弃旧图对象。需要强调一点，这里所说的抛弃只是不在容器submaps_中存放对象了，并不意味者对象就此销毁了， 因为容器是以共享指针的形式保存子图对象的。

        void ActiveSubmaps2D::FinishSubmap() {
            Submap2D* submap = submaps_.front().get();
            submap->Finish();
            ++matching_submap_index_;
            submaps_.erase(submaps_.begin());
        }

函数CreateGrid用于为子图创建栅格信息存储结构。如下面的代码所示，在获取了子图尺寸和分辨率信息之后，就构建了一个ProbabilityGrid类型的栅格存储。

        std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(const Eigen::Vector2f& origin) {
            constexpr int kInitialSubmapSize = 100;
            float resolution = options_.grid_options_2d().resolution();
            return common::make_unique<ProbabilityGrid>(MapLimits(resolution, origin.cast() + 0.5 * kInitialSubmapSize * resolution * Eigen::Vector2d::Ones(), CellLimits(kInitialSubmapSize, kInitialSubmapSize)));
        }

类ActiveSubmaps2D通过函数InsertRangeData将扫描数据插入到子图中。如下面的代码所示，先依次将输入参数range_data填进容器submaps_的子图中。然后，检查新图中插入的数据数量， 当超过配置项num_range_data的时候就会调用AddSubmap抛弃旧图创建新图。

        void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {
            for (auto& submap : submaps_)
                submap->InsertRangeData(range_data, range_data_inserter_.get());
            if (submaps_.back()->num_range_data() == options_.num_range_data())
                AddSubmap(range_data.origin.head<2>());
        }

此外，ActiveSubmaps2D还提供了函数matching_index和submaps分别用于获取当前的匹配子图索引和子图容器submaps_。

2. Submap2D

ActiveSubmaps2D中维护的子图数据类型是Submap2D， 该数据类型继承自Submap。父类Submap定义了2D和3D子图的一些共有的属性， 如下表所示，列出了该类定义的所有成员变量。该类主要描述了子图在全局坐标系下的相对位姿，记录插入的数据数量以及是否构建完成等基本信息。

而Submap2D类在它的父类的基础上只定义了一个成员变量"grid_"用于保存子图的具体内容。如下表所示。

Submap2D实际上有两个构造函数，这里我们只关心刚刚类ActiveSubmaps2D中所用的构造方式，其代码如下所示。该函数有两个参数，其中origin给出了子图的位置， 而grid则指示了子图实际用于保存数据的对象。因此，构造Submap2D之前，需要先提供一个Grid2D的对象。在构造函数中通过std::move赋予成员变量grid_。

        Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid)
            : Submap(transform::Rigid3d::Translation(Eigen::Vector3d(origin.x(), origin.y(), 0.))) {
            grid_ = std::move(grid);
        }

下面是父类Submap的构造函数，它除了设定了子图的位姿之外，什么也没做。

        Submap(const transform::Rigid3d& local_submap_pose) : local_pose_(local_submap_pose) {}

类Submap2D通过函数InsertRangeData将激光的扫描数据插入到grid_对象中，在调用该函数之前需要确保子图更新还没有结束。该函数有两个参数，其中range_data是将要插入的扫描数据， 而range_data_inserter则是一个辅助的工具，实际就是类ActiveSubmaps2D中的成员range_data_inserter_，具体负责插入数据的方式方法。

        void Submap2D::InsertRangeData(const sensor::RangeData& range_data, const RangeDataInserterInterface* range_data_inserter) {
            CHECK(grid_);
            CHECK(!finished());
            range_data_inserter->Insert(range_data, grid_.get());
            set_num_range_data(num_range_data() + 1);
        }

函数Finish负责终止子图的更新。在下面代码中的第4行通知grid_对象裁剪重叠的栅格，并在第5行中通过父类的接口set_finished更新完成标志。

        void Submap2D::Finish() {
            CHECK(grid_);
            CHECK(!finished());
            grid_ = grid_->ComputeCroppedGrid();
            set_finished(true);
        }

3. 完

通过分析类ActiveSubmaps2D，我们了解到在进行Local SLAM的时候，实际上只用到了"旧图"和"新图"两个子图。其中旧图用于扫描匹配，新图作为储备。 当新图中插入的数据达到一定程度之后，就替换旧图进行扫描匹配，并重新创建一个新图。

类Submap2D的对象就是我们一直说的子图。它通过Grid2D保存子图的详细数据，我们将在下一节中分析。此外Submap2D还记录了子图的位置、插入数据的数量、更新状态等信息。