Cartographer源码阅读2D&3D-删除Submap机制

Cartographer源码阅读-删除Submap机制

trim的删除接口在map_builder.cc文件中有所阐述。共包含两种trim的机制，第一种是OverlappingSubmapsTrimmer2D类型的，只用在2D SLAM中；第二种是PureLocalizationTrimmer，在2D和3D中都可以用。

删除机制的入口

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
  const int trajectory_id = trajectory_builders_.size();
  if (options_.use_trajectory_builder_3d()) {
    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_3d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(
          trajectory_options.trajectory_builder_3d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
    trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder3D(
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph3D*>(pose_graph_.get()),
                local_slam_result_callback)));
  } else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder2D(
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)));
    // 通过trajectory_options（lua配置文件中）：删除重复的submap，同时，设置的参数包括：fresh_submaps_count、min_covered_area、min_added_submaps_count三个参数
    if (trajectory_options.has_overlapping_submaps_trimmer_2d()) {
      const auto& trimmer_options =
          trajectory_options.overlapping_submaps_trimmer_2d();
      pose_graph_->AddTrimmer(common::make_unique<OverlappingSubmapsTrimmer2D>(
          trimmer_options.fresh_submaps_count(),
          trimmer_options.min_covered_area() /
              common::Pow2(trajectory_options.trajectory_builder_2d_options()
                               .submaps_options()
                               .grid_options_2d()
                               .resolution()),
          trimmer_options.min_added_submaps_count()));
    }
  }
  // 通过trajectory_options（lua配置文件中）：纯定位模式下，只保留最新的3个submap，不需要其他额外参数
  if (trajectory_options.pure_localization()) {
    constexpr int kSubmapsToKeep = 3;
    pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
        trajectory_id, kSubmapsToKeep));
  }
  if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }
  proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
  for (const auto& sensor_id : expected_sensor_ids) {
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }
  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;
  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
  CHECK_EQ(trajectory_builders_.size(, all_trajectory_builder_options_.size());
  return trajectory_id;
}

抽象类

class Trimmable {
 public:
  virtual ~Trimmable() {}
  // submap数量
  virtual int num_submaps(int trajectory_id) const = 0;
  // submap id
  virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
  // Returns finished submaps with optimized poses only.
  virtual MapById<SubmapId, PoseGraphInterface::SubmapData>
  GetOptimizedSubmapData() const = 0;
  virtual const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const = 0;
  virtual const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
      const = 0;

  // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They
  // will no longer take part in scan matching, loop closure, visualization.
  // Submaps and nodes are only marked, the numbering remains unchanged.
  // 标记submap为可删除
  virtual void MarkSubmapAsTrimmed(const SubmapId& submap_id) = 0;

  // Checks if the given trajectory is finished or not.
  virtual bool IsFinished(int trajectory_id) const = 0;
};

class PoseGraphTrimmer {
 public:
  virtual ~PoseGraphTrimmer() {}

  // Called once after each pose graph optimization.
  virtual void Trim(Trimmable* pose_graph) = 0;

  // Checks if this trimmer is in a terminatable state.
  virtual bool IsFinished() = 0;
};

submap删除的具体实现

在pose_graph_2d.h中继承了Trimmable类：且用this指针构造TrimmingHandle

class TrimmingHandle : public Trimmable {
   public:
    TrimmingHandle(PoseGraph2D* parent);
    ~TrimmingHandle() override {}

    int num_submaps(int trajectory_id) const override;
    std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
    MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
        REQUIRES(parent_->mutex_);
    const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
        REQUIRES(parent_->mutex_);
    const std::vector<Constraint>& GetConstraints() const override
        REQUIRES(parent_->mutex_);
    void MarkSubmapAsTrimmed(const SubmapId& submap_id)
        REQUIRES(parent_->mutex_) override;
    bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);

   private:
    PoseGraph2D* const parent_;
  };

PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent)
    : parent_(parent) {}
// 获取当前的submap数量
int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
  const auto& submap_data = parent_->optimization_problem_->submap_data();
  return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
//  获取优化后的submap数据
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const {
  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
  for (const auto& submap_id_data : parent_->submap_data_) {
    if (submap_id_data.data.state != SubmapState::kFinished ||
        !parent_->global_submap_poses_.Contains(submap_id_data.id)) {
      continue;
    }
    submaps.Insert(submap_id_data.id,
                   SubmapData{submap_id_data.data.submap,
                              transform::Embed3D(parent_->global_submap_poses_
                                                     .at(submap_id_data.id)
                                                     .global_pose)});
  }
  return submaps;
}

void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
    const SubmapId& submap_id) {
  // TODO(hrapp): We have to make sure that the trajectory has been finished
  // if we want to delete the last submaps.
  // 不允许删除没有finish的submap
  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);

  // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
  // with 'submap_id' is gone.
  // 获取所有需要保留的NodeId
  std::set<NodeId> nodes_to_retain;
  for (const Constraint& constraint : parent_->constraints_) {
    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
        constraint.submap_id != submap_id) {
      nodes_to_retain.insert(constraint.node_id);
    }
  }
  // Remove all 'constraints_' related to 'submap_id'.
  std::set<NodeId> nodes_to_remove;
  {
    std::vector<Constraint> constraints;
    for (const Constraint& constraint : parent_->constraints_) {
      if (constraint.submap_id == submap_id) {
        // 获取所有需要删除的node
        if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
            nodes_to_retain.count(constraint.node_id) == 0) {
          // This node will no longer be INTRA_SUBMAP contrained and has to be
          // removed.
          nodes_to_remove.insert(constraint.node_id);
        }
      } else {
        // 获取所有和待删除的submap不相关的约束
        constraints.push_back(constraint);
      }
    }
    parent_->constraints_ = std::move(constraints);
  }
  // Remove all 'constraints_' related to 'nodes_to_remove'.
  {
    std::vector<Constraint> constraints;
    for (const Constraint& constraint : parent_->constraints_) {
      if (nodes_to_remove.count(constraint.node_id) == 0) {
        // 获取所有不和待删除Node相关的约束
        constraints.push_back(constraint);
      }
    }
    // 赋值给pose_graph，相当于删掉了所有和要删除的（Node和submap）相关的约束
    parent_->constraints_ = std::move(constraints);
  }

  // Mark the submap with 'submap_id' as trimmed and remove its data.
  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
  // 删除submap
  parent_->submap_data_.Trim(submap_id);
  // 删除submap对应的scan matcher
  parent_->constraint_builder_.DeleteScanMatcher(submap_id);
  // 后端优化类内删除对应的submap
  parent_->optimization_problem_->TrimSubmap(submap_id);

  // Remove the 'nodes_to_remove' from the pose graph and the optimization
  // problem.
  for (const NodeId& node_id : nodes_to_remove) {
    // pose_graph内删除node
    parent_->trajectory_nodes_.Trim(node_id);
    // 后端优化类内删除对应的node
    parent_->optimization_problem_->TrimTrajectoryNode(node_id);
  }
}

// 获取submap id
std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
    int trajectory_id) const {
  std::vector<SubmapId> submap_ids;
  // 获取所有轨迹的submap id
  const auto& submap_data = parent_->optimization_problem_->submap_data();
  // 只获取当前轨迹的submap id
  for (const auto& it : submap_data.trajectory(trajectory_id)) {
    submap_ids.push_back(it.id);
  }
  return submap_ids;
}
// 获取Node
const MapById<NodeId, TrajectoryNode>&
PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
  return parent_->trajectory_nodes_;
}
// 获取约束
const std::vector<PoseGraphInterface::Constraint>&
PoseGraph2D::TrimmingHandle::GetConstraints() const {
  return parent_->constraints_;
}

bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
  return parent_->IsTrajectoryFinished(trajectory_id);
}

PureLocalizationTrimmer

只保留最新的三个submap

class PureLocalizationTrimmer : public PoseGraphTrimmer {
 public:
  PureLocalizationTrimmer(int trajectory_id, int num_submaps_to_keep);
  ~PureLocalizationTrimmer() override {}

  void Trim(Trimmable* pose_graph) override;
  bool IsFinished() override;

 private:
  const int trajectory_id_;
  int num_submaps_to_keep_;
  bool finished_ = false;
};
PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
                                                 const int num_submaps_to_keep)
    : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) {
  // 至少保留3个submap，保留数量需要传参
  CHECK_GE(num_submaps_to_keep, 3);
}

void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
  // 如果pose_graph结束，则不再删除submap，该pose_graph为传参进来的Trimmable
  if (pose_graph->IsFinished(trajectory_id_)) {
    num_submaps_to_keep_ = 0;
  }
  // 获取submap id
  auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
  // 保留最新的三个submap 
  for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) {
    pose_graph->MarkSubmapAsTrimmed(submap_ids.at(i));
  }

  if (num_submaps_to_keep_ == 0) {
    finished_ = true;
  }
}

bool PureLocalizationTrimmer::IsFinished() { return finished_; }

OverlappingSubmapsTrimmer2D

原理比较简单，不用细细展开。

class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
 public:
  OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count,
                              uint16 min_covered_cells_count,
                              uint16 min_added_submaps_count)
      : fresh_submaps_count_(fresh_submaps_count),
        min_covered_cells_count_(min_covered_cells_count),
        min_added_submaps_count_(min_added_submaps_count) {}
  ~OverlappingSubmapsTrimmer2D() override = default;

  void Trim(Trimmable* pose_graph) override;
  bool IsFinished() override { return finished_; }

 private:
  // Number of the most recent submaps to keep.
  const uint16 fresh_submaps_count_;
  // Minimal number of covered cells to keep submap from trimming.
  const uint16 min_covered_cells_count_;
  // Number of added submaps before the trimmer is invoked.
  const uint16 min_added_submaps_count_;
  // Current finished submap count.
  uint16 current_submap_count_ = 0;

  bool finished_ = false;
};

void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
  // 获取优化后的所有submap数据
  const auto submap_data = pose_graph->GetOptimizedSubmapData();
  // 设置的参数相关：每满min_added_submaps_count_个新的submap，执行一次删除
  if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) {
    return;
  }
  // 获取第一个submap的网格信息
  const MapLimits first_submap_map_limits =
      std::static_pointer_cast<const Submap2D>(submap_data.begin()->data.submap)
          ->grid()
          ->limits();
  // 构造SubmapCoverageGrid2D类
  SubmapCoverageGrid2D coverage_grid(first_submap_map_limits);
  // 计算每个submap内最后一个node的时间
  const std::map<SubmapId, common::Time> submap_freshness =
      ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(),
                             pose_graph->GetConstraints());
  // 填充coverage_grid
  const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
      submap_freshness, submap_data, &coverage_grid);
  // 找到待删除的所有submap id
  const std::vector<SubmapId> submap_ids_to_remove =
      FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
                          min_covered_cells_count_);
  current_submap_count_ = submap_data.size() - submap_ids_to_remove.size();
  // 逐个删除submap
  for (const SubmapId& id : submap_ids_to_remove) {
    pose_graph->MarkSubmapAsTrimmed(id);
  }
}


std::map<SubmapId, common::Time> ComputeSubmapFreshness(
    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
    const MapById<NodeId, TrajectoryNode>& trajectory_nodes,
    const std::vector<PoseGraphInterface::Constraint>& constraints) {
  std::map<SubmapId, common::Time> submap_freshness;

  // Find the node with the largest NodeId per SubmapId.
  // 根据约束，寻找submap内所有的Node_id
  std::map<SubmapId, NodeId> submap_to_latest_node;
  // 遍历所有的约束
  for (const PoseGraphInterface::Constraint& constraint : constraints) {
    // 只遍历sequence边
    if (constraint.tag != PoseGraphInterface::Constraint::INTRA_SUBMAP) {
      continue;
    }
    auto submap_to_node = submap_to_latest_node.find(constraint.submap_id);
    if (submap_to_node == submap_to_latest_node.end()) {
      submap_to_latest_node.insert(
          std::make_pair(constraint.submap_id, constraint.node_id));
      continue;
    }
    // 找到最新的node_id
    submap_to_node->second =
        std::max(submap_to_node->second, constraint.node_id);
  }

  // Find timestamp of every latest node.
  // 遍历所有的submap及其对应的最新的Node_id
  for (const auto& submap_id_to_node_id : submap_to_latest_node) {
    auto submap_data_item = submap_data.find(submap_id_to_node_id.first);
    // 需要能找到该submap id
    if (submap_data_item == submap_data.end()) {
      LOG(WARNING) << "Intra-submap constraint between SubmapID = "
                   << submap_id_to_node_id.first << " and NodeID "
                   << submap_id_to_node_id.second << " is missing submap data";
      continue;
    }
    auto latest_node_id = trajectory_nodes.find(submap_id_to_node_id.second);
    if (latest_node_id == trajectory_nodes.end()) continue;
    // 将Node时间取出作为submap时间
    submap_freshness[submap_data_item->id] = latest_node_id->data.time();
  }
  return submap_freshness;
}

// 将所有的submap中的所有点都投影到coverage_grid中
std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
    const std::map<SubmapId, common::Time>& submap_freshness,
    const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
    SubmapCoverageGrid2D* coverage_grid) {
  std::set<SubmapId> all_submap_ids;
  
  for (const auto& submap : submap_data) {
    auto freshness = submap_freshness.find(submap.id);
    if (freshness == submap_freshness.end()) continue;
    if (!submap.data.submap->finished()) continue;
    all_submap_ids.insert(submap.id);
    // 获取submap网格
    const Grid2D& grid =
        *std::static_pointer_cast<const Submap2D>(submap.data.submap)->grid();
    // Iterate over every cell in a submap.
    // 裁剪submap，因为里面有未经裁剪的submap
    Eigen::Array2i offset;
    CellLimits cell_limits;
    grid.ComputeCroppedLimits(&offset, &cell_limits);
    if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
      LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
      continue;
    }
    // submap在global SLAM坐标系下的位姿
    const transform::Rigid3d& global_frame_from_submap_frame = submap.data.pose;
    // submap在Local SLAM坐标系下的位姿的逆
    const transform::Rigid3d submap_frame_from_local_frame =
        submap.data.submap->local_pose().inverse();
    // 遍历所有的点
    for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
      const Eigen::Array2i index = xy_index + offset;
      if (!grid.IsKnown(index)) continue;
      // 根据cell id，转换为3d坐标(在Local SLAM坐标系下的坐标)
      const transform::Rigid3d center_of_cell_in_local_frame =
          transform::Rigid3d::Translation(Eigen::Vector3d(
              grid.limits().max().x() -
                  grid.limits().resolution() * (index.y() + 0.5),
              grid.limits().max().y() -
                  grid.limits().resolution() * (index.x() + 0.5),
              0));
      // 点在global SLAM坐标系下的位姿
      const transform::Rigid2d center_of_cell_in_global_frame =
          transform::Project2D(global_frame_from_submap_frame *
                               submap_frame_from_local_frame *
                               center_of_cell_in_local_frame);
      coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
                              submap.id, freshness->second);
    }
  }
  return all_submap_ids;
}

std::vector<SubmapId> FindSubmapIdsToTrim(
    const SubmapCoverageGrid2D& coverage_grid,
    const std::set<SubmapId>& all_submap_ids, uint16 fresh_submaps_count,
    uint16 min_covered_cells_count) {
  // 对于每个cell保留最新的fresh_submaps_count个观测，记录每个观测中相同submap_id的保留数量
  std::map<SubmapId, uint16> submap_to_covered_cells_count;
  // 遍历coverage_grid的所有cells
  for (const auto& cell : coverage_grid.cells()) {
    // 取出该cell的所有观测
    std::vector<std::pair<SubmapId, common::Time>> submaps_per_cell(
        cell.second);

    // In case there are several submaps covering the cell, only the freshest
    // submaps are kept.
    // 如果观测数量大于设定的阈值fresh_submaps_count，则保留最新的fresh_submaps_count个观测
    if (submaps_per_cell.size() > fresh_submaps_count) {
      // Sort by time in descending order.
      std::sort(submaps_per_cell.begin(), submaps_per_cell.end(),
                [](const std::pair<SubmapId, common::Time>& left,
                   const std::pair<SubmapId, common::Time>& right) {
                  return left.second > right.second;
                });
      submaps_per_cell.erase(submaps_per_cell.begin() + fresh_submaps_count,
                             submaps_per_cell.end());
    }
    for (const std::pair<SubmapId, common::Time>& submap : submaps_per_cell) {
      ++submap_to_covered_cells_count[submap.first];
    }
  }
  std::vector<SubmapId> submap_ids_to_keep;
  for (const auto& id_to_cells_count : submap_to_covered_cells_count) {
    // 如果该submap被保留的数量大于预设值min_covered_cells_count，则保留该submap
    if (id_to_cells_count.second < min_covered_cells_count) continue;
    submap_ids_to_keep.push_back(id_to_cells_count.first);
  }

  DCHECK(std::is_sorted(submap_ids_to_keep.begin(), submap_ids_to_keep.end()));
  std::vector<SubmapId> result;
  // 求集合的差集，得到待删除的所有submap_id
  std::set_difference(all_submap_ids.begin(), all_submap_ids.end(),
                      submap_ids_to_keep.begin(), submap_ids_to_keep.end(),
                      std::back_inserter(result));
  return result;
}

SubmapCoverageGrid2D

class SubmapCoverageGrid2D {
 public:
  // Aliases for documentation only (no type-safety).
  using CellId = std::pair<int64 /* x cells */, int64 /* y cells */>;
  using StoredType = std::vector<std::pair<SubmapId, common::Time>>;

  SubmapCoverageGrid2D(const MapLimits& map_limits)
      : offset_(map_limits.max()), resolution_(map_limits.resolution()) {}

  void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id,
                const common::Time& time) {
    CellId cell_id{common::RoundToInt64((offset_(0) - point(0)) / resolution_),
                   common::RoundToInt64((offset_(1) - point(1)) / resolution_)};
    cells_[cell_id].emplace_back(submap_id, time);
  }

  const std::map<CellId, StoredType>& cells() const { return cells_; }

 private:
  // 第一个submap的最大xy(对应的cell id为0，0)
  Eigen::Vector2d offset_;
  // 地图分辨率
  double resolution_;
  // 存放所有cell的id,及id对对应的所有观测（submap的id，及submap的时间，该时间为submap第一帧激光的时间）
  std::map<CellId, StoredType> cells_;
};