Cartographer源码阅读---番外篇: Submap封装与维护

Cartographer中Submap(子图)没有被直接的调用进行维护, 而是针对2D和3D场景分别派生出子类Submap2D和Submap3D, 进行调用. 以2D为例, 为了方便维护, 又把Submap2D封装成了ActiveSubmaps2D进行维护, 其维护方式类似与滑窗, 也是只维护最近的一些数据.

1. Submap类

/**
 * @brief 独立的子地图, 3个功能
 * 
 * 保存在local坐标系下的子图的坐标
 * 记录插入到子图中雷达数据的个数
 * 标记这个子图是否是完成状态
 */
class Submap {
 public:

  // 构造函数, 将传入的local_submap_pose作为子图的坐标原点
  Submap(const transform::Rigid3d& local_submap_pose)
      : local_pose_(local_submap_pose) {}
  virtual ~Submap() {}

  virtual proto::Submap ToProto(bool include_grid_data) const = 0;
  virtual void UpdateFromProto(const proto::Submap& proto) = 0;

  // Fills data into the 'response'.
  virtual void ToResponseProto(
      const transform::Rigid3d& global_submap_pose,
      proto::SubmapQuery::Response* response) const = 0;

  // Pose of this submap in the local map frame.
  // 在local坐标系的子图的坐标
  transform::Rigid3d local_pose() const { return local_pose_; }

  // Number of RangeData inserted.
  // 插入到子图中雷达数据的个数
  int num_range_data() const { return num_range_data_; }
  void set_num_range_data(const int num_range_data) {
    num_range_data_ = num_range_data;
  }

  bool insertion_finished() const { return insertion_finished_; }
  // 将子图标记为完成状态
  void set_insertion_finished(bool insertion_finished) {
    insertion_finished_ = insertion_finished;
  }

 private:
  const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标
  int num_range_data_ = 0;
  bool insertion_finished_ = false;
};

从私有变量可以看到, Submap维护了三个东西

1. 自己的坐标, 这个坐标是在local坐标系下的坐标
2. 本子图中节点的个数, 也就是有几帧雷达数据
3. 是不是个结束插入的子图, 节点数大于一定值后就不能在插入雷达数据了

Submap的坐标系在构造的时候就得到了, 是子图第一帧激光雷达的坐标系, 分析如下
Submap的第一次构造在

  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

中, 调用的是active_submap的InsertRangeData这个函数.
在submap_2d.cc中看 ActiveSubmaps2D::InsertRangeData这个函数,

// 将点云数据写入到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());  // 是submap类的, 不是ActiveSubmaps2D的InsertRangeData
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { //一个子图有180个扫描
    submaps_.front()->Finish();
  }
  return submaps();
}

其中新插入子图是调用了AddSubmap(range_data.origin.head<2>()),

// 新增一个子图,根据子图个数判断是否删掉第一个子图
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_));
}

也就是把range_data.origin.head<2>()放进来作为submap的原点, 再看看range_data

/**
 * @brief local_slam_data中存储所有雷达点云的数据结构
 * 
 * @param origin  点云的原点在local坐标系下的坐标
 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
 * @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
 */
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses; // local坐标系下的坐标
};

origin就是这个点云在local坐标系下的坐标.
所以说submap的坐标就是submap第一帧点云点在local坐标系下的坐标.

2. submap2D类

…

3. ActiveSubmap2D类

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

// 将点云数据写入到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());  // 是submap类的, 不是ActiveSubmaps2D的InsertRangeData
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { //一个子图有180个扫描
    submaps_.front()->Finish();
  }
  return 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_));
}

…