cartographer 代码思想解读（6）- submap封装与维护

最新推荐文章于 2025-02-10 21:34:08 发布

jiajiading

最新推荐文章于 2025-02-10 21:34:08 发布

阅读量3.5k

点赞数 6

分类专栏： slam

本文链接：https://blog.youkuaiyun.com/jiajiading/article/details/108708711

版权

slam 专栏收录该内容

29 篇文章

订阅专栏

cartographer 代码思想解读（6）- submap封装与维护

前面两节描述了submap2d的存储形式、表示形式、概率地图、继承关系，同时也包含了submap具体的初始化、更新原理和过程，即算法核心具体实现。这节主要描述下cartographer如何在代码中进行封装和调用的。cartographer代码中并没有直接调用submap的类进行处理，而是将submap类封装成submap2d进行调用，同时将其submap2d重新封装成了ActiveSubmaps2D类进行维护和管理。
其代码目录如下：
cartographer/mapping/2d/submap2d.h

ActiveSubmaps2D类实际是对submap在使用过程进行了特殊管理和操作，其内部包含了两个submap2D，一个用于匹配，一个用于插入。其类定义如下：

// ActiveSubmaps2D类维护了两个submap,一个old 和一个new，
// old用于matching，而new用于插入
// 当new submap达到一定数量时，则停止插入。
// 抛弃old，将new变为old
// 创建一个新的new submap
class ActiveSubmaps2D {
 public:
  explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options);

  ActiveSubmaps2D(const ActiveSubmaps2D&) = delete;
  ActiveSubmaps2D& operator=(const ActiveSubmaps2D&) = delete;

  // Inserts 'range_data' into the Submap collection.
  std::vector<std::shared_ptr<const Submap2D>> InsertRangeData(
      const sensor::RangeData& range_data);

  std::vector<std::shared_ptr<const Submap2D>> submaps() const;

 private:
  // 创建插入器接口
  std::unique_ptr<RangeDataInserterInterface> CreateRangeDataInserter();
  // 创建grid地图接口
  std::unique_ptr<GridInterface> CreateGrid(const Eigen::Vector2f& origin);
  void FinishSubmap();    //无用，估计是代码升级后，删除的方法
  // 加入一个新的submap
  void AddSubmap(const Eigen::Vector2f& origin);

  const proto::SubmapsOptions2D options_;                              // 配置信息
  std::vector<std::shared_ptr<Submap2D>> submaps_;                     // 维护submap2d的列表
  std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;    // 插入器接口
  ValueConversionTables conversion_tables_;                            // 浮点数与到uint16转换表格

ActiveSubmaps2D类中的submaps_列表实际最多只两个submap，一个认为是old_map，另一个认为是new_map，类似于滑窗操作。当new_map插入激光scan的个数达到阈值时，则会将old_map进行结束，并且不再增加新的scan。同时将old_map进行删除，将new_map作为oldmap，然后重新初始化一个新的submap作为newmap。其具体实现可看代码注解，较为简单。

//ActiveSubmaps2D插入一个新的rangedata
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  // 如果第一次，即无任何submap2d时
  // 或者如果new的submap的内部含有的激光个数达到配置的阈值
  // 则需要增加一个新的submap，其submap2d的初始位置，为新range的激光坐标
  // 注意这是在插入前先进行了判断，也就说上次循环已经满足阈值条件
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }

  //两个submap全部插入新的range_data，表明old的submap2d包含了所有new submap2d内容
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  // 如果old的range的数量达到配置的阈值两倍，也表明new的数量也到达了阈值
  // 则 将old的submap进行结束封装，表明submap结束，设置submap2d 结束标志位，同时也进行裁剪仅保留有效value的边界
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  return submaps();
}

// 添加一个新的submap
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  // 如果已经存在两个submap，即old和new均存在
  // 则剔除old的submap2d
  if (submaps_.size() >= 2) {
    // This will crop the finished Submap before inserting a new Submap to
    // reduce peak memory usage a bit.
    // 等待插入结束标志位，即也是submap裁剪结束标志
    CHECK(submaps_.front()->insertion_finished());
    // 剔除old的submap2d
    submaps_.erase(submaps_.begin());
  }
  // 插入一个新的submap2d
  submaps_.push_back(absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));
}

简单总结：
ActiveSubmaps2D类是cartographer中正在处理的submap地图顶层封装，可设置、获取、更新、插入、初始化当前submap，其主要封装了一个类似滑窗的方法，两个submap，一旧一新，其更新过程可如下图例，其实也可以看出每个相邻的两个submap实际上是有N个过渡的激光帧。
在这里插入图片描述
cartographer与其他slam最大的一个不同点就是采用了submap概念，即采用一定数量的laser scan组成submap，然后再由n个submap组成整个地图空间。其中ActiveSubmaps2D类中旧的submap当采集完成时，即作为真正的存储的submap。