(02)Cartographer源码无死角解析-(41) 2D栅格地图→ActiveSubmaps2D

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过前面一系列的博客，PoseExtrapolator 进行了比较细致的分析。到目前为止，对于点云数据的预处理过程可以说时十分了解了，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。

在讲解扫描匹配之前，先来看看 Cartographer 2D 的栅格地图，其不像3D点云地图有很多成熟的库可以调用，具有统一的标准。大多数 2D Slam 的栅格地图都是需要自己编写代码进行构建的。下面就来看看 Cartographer 中时如何构建的。

关于2D栅格地图的构建主要涉及到如下几个类，后续会分别对齐进行详细的讲解：

ActiveSubmaps2D 
Submap2D    Submap 子父类关系
Grid2D    ProbabilityGrid  子父类关系
RangeDataInserterInterface   ProbabilityGridRangeDataInserter2D//子父类关系

二、类间关系

先来看看源码中时如何把这些类，或者类对象关联起来的

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
src/cartographer/configuration_files/trajectory_builder_2d.lua

2、Submap2D 与 Submap

类 ActiveSubmaps2D 包含成员变量 Submap2D ，如下所示：

扫描二维码关注公众号，回复： 14550973 查看本文章

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
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 = {
    
    
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",  --使用2D栅格概率图插入数据
      -- 概率占用栅格地图的一些配置
      probability_grid_range_data_inserter = {
    
    
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      -- tsdf地图的一些配置
      tsdf_range_data_inserter = {
    
    
		......
		......
      },
    },

这些参数的具体作用，后续再做详细的讲解。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() 函数从命名可以看出，其主要功能为插入雷达数据到子图中，流程如下：

$\color{blue}(01)$ 如果submaps_ 不存在任何一个子图，或者 submaps_ 中最后一个子图数据的数量达到了与 options_.num_range_data()=90(默认配置)，则调用 ActiveSubmaps2D::AddSubmap() 函数新建一个子图。

$\color{blue}(02)$ 将一帧雷达数据 range_data 写入到所有子图之中 submaps_ 。不过注意，通常 submaps_ 最多只包含两个子图。具体原由稍后讲解原由。

$\color{blue}(03)$ 如果 submaps_ 中第一个子图中场插入的数据数量达到了两倍 options_.num_range_data()，则把该子图标记为完成。

$\color{blue}(04)$ 调用 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());
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    
    
    submaps_.front()->Finish();
  }
  return submaps();
}

五、AddSubmap()

上面提到，如果submaps_ 不存在任何一个子图，或者 submaps_ 中最后一个子图数据的数量达到了与 options_.num_range_data()=90(默认配置)，则调用 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());
    // 删掉第一个子图的指针
    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() 数据集时，子图二才被创建，同时会删除 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也存在交集，依次循环下去。

最后了，会保证两个相邻的子图之间是存在共同数据的，其目的是为了点云匹配时，在两个子图间出现断层的现象。具体的细节后续再做更加详细的分析。