转自“(02)Cartographer源码无死角解析-(41) 2D栅格地图→ActiveSubmaps2D”
一、前言
LocalTrajectoryBuilder2D得到此次在odom坐标系下的点云range_data_in_local后，接下要把点云画向子图（Submap2D）。当系统中已有的子图数>=2时，一个点云会同时画向最新的那两张子图。
子图只会在ActiveSubmaps2D创建，具体在ActiveSubmaps2D::AddSubmap。但ActiveSubmaps2D最多存储两个子图，一旦超过两个了，就会删掉一个最老的。这里删掉，不是说这子图就没了，只是将它的引用计数减1。这子图还有其它地方存在的。
二、类间关系
先来看看源码中时如何把这些类，或者类对象关联起来的
1、ActiveSubmaps2D
在 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数中，可以找到如下代码：
// 将校正后的点云写入submap std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap( time, range_data_in_local, filtered_gravity_aligned_point_cloud, pose_estimate, gravity_alignment.rotation());
该处对InsertIntoSubmap()函数的调用，起到了与类 ActiveSubmaps2D 的交互。因为 LocalTrajectoryBuilder2D::InsertIntoSubmap() 函数中，使用到类 LocalTrajectoryBuilder2D 的成员对象：
ActiveSubmaps2D active_submaps_;
该成员对象在 LocalTrajectoryBuilder2D 构造函数的初始化列表中被赋予初值，初始化列表可以看到如下代码
active_submaps_(options.submaps_options())
其首先根据配置文件中的 submaps 信息，构建 ActiveSubmaps2D 对象，然后赋值给 active_submaps_。配置文件路径为：
src/cartographer/configuration_files/trajectory_builder_2d.lua
2、Submap2D 与 Submap
类 ActiveSubmaps2D 包含成员变量 Submap2D ，如下所示：
std::vector<std::shared_ptr<Submap2D>> submaps_;
另外 Submap2D 为 Submap 的派生类，其都实现于文件 src/cartographer/cartographer/mapping/2d/submap_2d.cc 之中，Submap2D 存在两个重载构造函数，其都会构件 Grid2D 实例对象，然后赋值给成员变量：
std::unique_ptr<Grid2D> grid_; // 地图栅格数据
3、Grid2D
Grid2D 继承自 src/cartographer/cartographer/mapping/grid_interface.h 文件中的 GridInterface，GridInterface 比较简单，仅仅几句代码而已。另外 Grid2D 包含成员变量 MapLimits limits_。另外，Grid2D 同时也是一个基类，如 ProbabilityGrid 与 TSDF2D 都为其派生类。
4、ProbabilityGrid
通过 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的 ActiveSubmaps2D::AddSubmap() 函数，可以得知在构建 Grid2D 对象的时候，其调用的 ActiveSubmaps2D::CreateGrid() 函数，该函数会根据不同的配置信息构件 ProbabilityGrid 或者是 TSDF2D 对象。
5、ProbabilityGridRangeDataInserter2D
调用 Submap2D::InsertRangeData() 函数，其需要传递一个 RangeDataInserterInterface 类型的指针对象，从命名可以看出其为一个接口，源码中实际上传入的实参对象由 ActiveSubmaps2D::CreateRangeDataInserter() 函数决定，其派生类 ProbabilityGridRangeDataInserter2D 与 TSDFRangeDataInserter2D 主要是负责数据插入的功能。ActiveSubmaps2D::CreateRangeDataInserter() 函数也是在 ActiveSubmaps2D 的构造函数中被调用。
三、ActiveSubmaps2D
首先我们来看看 ActiveSubmaps2D 这个类，其主要负责与 LocalTrajectoryBuilder2D 的交互，同时内部再通过 Submap2D、Submap 、Grid2D 、ProbabilityGrid 、ProbabilityGrid 、ProbabilityGridRangeDataInserter2D 这几个类完成地图的保存与插入。
前面已经介绍过，ActiveSubmaps2D 的实例对象在 LocalTrajectoryBuilder2D 构造函数中根据：
src/cartographer/configuration_files/trajectory_builder_2d.lua
配置文件中的如下参数进行构件：
-- 子图相关的一些配置
submaps = {
num_range_data = 90, -- 一个子图里插入点云的个数的一半
grid_options_2d = {
grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式
resolution = 0.05, --分辨率
},
range_data_inserter = {
--使用2D栅格概率图插入数据
range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
-- 概率占用栅格地图的一些配置
probability_grid_range_data_inserter = {
insert_free_space = true,
hit_probability = 0.55,
miss_probability = 0.49,
},
-- tsdf地图的一些配置
tsdf_range_data_inserter = {
......
......
},
},rose_ros使用revo_lds.lua做为“-configuration_basename”参数值，当中出现同名配置时会覆盖掉trajectory_builder_2d.lua中的，目前sutmaps块内的就一项。
<apps-res>/data/core/cert/cartographer_ros/configuration_files/revo_lds.lua ------ TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
这些参数的具体作用，后续再做详细的讲解。ActiveSubmaps2D 主要有如下几个成员变量：
const proto::SubmapsOptions2D options_; std::vector<std::shared_ptr<Submap2D>> submaps_; std::unique_ptr<RangeDataInserterInterface> range_data_inserter_; // 转换表, 第[0-32767]位置, 存的是[0.9, 0.1~0.9]的数据 ValueConversionTables conversion_tables_;
options_ 主要存储匹配信息，submaps_ 用于存储多个子图，range_data_inserter_ 与 ValueConversionTables 后续进行详细分析。下面来分析 ActiveSubmaps2D 的成员函数。
四、InsertRangeData()
对于 ActiveSubmaps2D::InsertRangeData() 函数从命名可以看出，其主要功能为插入点云到子图中，流程如下：
- 如果submaps_ 不存在任何一个子图，或者 submaps_ 中最后一个子图画了的点云个数达到了options_.num_range_data()=35，则调用 ActiveSubmaps2D::AddSubmap() 函数新建一个子图。
- 将一帧点云range_data 写入到所有子图之中 submaps_ 。不过注意，通常 submaps_ 最多只包含两个子图。具体原由稍后讲解原由。
- 如果 submaps_ 中第一个子图中场插入的数据数量达到了两倍 options_.num_range_data()，则把该子图标记为完成。
- 调用 ActiveSubmaps2D::submaps() 函数，使用共享指针返回 submaps_ 中的所有子图。
代码注释如下：
// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
const sensor::RangeData& range_data) {
// 如果第二个子图插入点云个数等于num_range_data时,就新建个子图
// 因为这时第一个子图应该已经处于完成状态了
if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
// 将一帧点云同时写入两个子图中
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_.get());
}
// old_submap的点云个数等于2倍的num_range_data时，new_submap的点云个数应该等于num_range_data
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
submaps_.front()->Finish();
}
return submaps();
}
五、AddSubmap()
上面提到，如果submaps_ 不存在任何一个子图，或者 submaps_ 中new_submap中点云个数达到了options_.num_range_data()=35，则调用 ActiveSubmaps2D::AddSubmap() 函数新建一个子图。
新建子图调用的函数就是 ActiveSubmaps2D::AddSubmap()，其目的是构造一个 Submap2D 独占指针对象，然后添加到 submaps_ 之后，不过有几个点是需要注意的：
如果 submaps_ 中包含的子图数量，即 submaps_.size() 大于等于 2，那么会删掉 submaps_ 中的第一个地图。所以与前面的内容就呼应起来了，submaps_中最多存在两个子图。因为若 submaps_ 已经存在两个及两个以上的子图时，新建一个子图的同时会删除一个子图，所以依旧为两个子图。
代码注释如下：
// 新增一个子图,根据子图个数判断是否删掉第一个子图
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
// 调用AddSubmap时第一个子图一定是完成状态,所以子图数为2时就可以删掉第一个子图了
if (submaps_.size() >= 2) {
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
CHECK(submaps_.front()->insertion_finished());
// 删掉第一个子图的指针。这里删掉，不是说这子图就没了，只是将它的引用计数减1。
// 这子图还有其它地方存在的。
submaps_.erase(submaps_.begin());
}
// 新建一个子图, 并保存指向新子图的智能指针
submaps_.push_back(absl::make_unique<Submap2D>(
origin,
std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(CreateGrid(origin).release())),
&conversion_tables_));
}
六、CreateRangeDataInserter()
ActiveSubmaps2D 可以支持 概率栅格地图 与 tsdf地图，通过 ActiveSubmaps2D::CreateRangeDataInserter() 函数，根据配置信息可以构建 ProbabilityGridRangeDataInserter2D 与 TSDFRangeDataInserter2D 对象。本人使用的是 ProbabilityGridRangeDataInserter2D，所以后续以其为例进行讲解。他们都派生自 RangeDataInserterInterface()，主要实现如下纯虚函数，用于插入点云(后续会做详细讲解)。
// Inserts 'range_data' into 'grid'. virtual void Insert(const sensor::RangeData& range_data,GridInterface* grid) const = 0;
七、CreateGrid()
ActiveSubmaps2D::CreateGrid() 函数，主要是根据雷达传感器的原点构建 ProbabilityGrid 或者 TSDF2D 对象，主要作用进行地图保存，其都继承于 Grid2D。另外需要注意，在构建 ProbabilityGrid 或者 TSDF2D 时，会构建一个 MapLimits 对象当作实参传入到构造函数。
八、结语
在 ActiveSubmaps2D 的这些成员函数中，不难看出，对于子图的相关处理都集中在成员函数InsertRangeData()函数，其还调用了另一个重要函数AddSubmap()，为了方便理解，根据下图进行讲解一下：
其上的每个方形代表一个子图 Submap2D，根据上面的分析知道submaps_最多同时存在两个子图，这里假设现在submaps_中存储的子图为子图1与子图2。那么子图1中插入的数据定然比子图2多options_.num_range_data()个点云，因为只有子图一达到options_.num_range_data()个点云时，子图2才被创建，同时会删除submaps_最前面的子图（这里假设为子图0，未在上图画出）。后续到来的点云会同时插入到子图1与子图2，也就是说，这两个子图的点云是存在交集的。
当子图2点云达到了options_.num_range_data()个，也就是此时子图1的数据为2倍的options_.num_range_data()，会把子图1标记为完成状态，同时从 submaps_ 中删除该子图，这样子图2代替了之前子图1的位置，同时会再创建子图3添加到 submaps_ 之中。也就是说，此时submaps_中包含了子图2与子图3，然后再继续往子图2,3插入数据，所以子图2与子图3也存在交集，依次循环下去。
最后了，会保证两个相邻的子图之间是存在共同数据的，其目的是为了点云匹配时，在两个子图间出现断层的现象。具体的细节后续再做更加详细的分析。