cartographer之TrimSubmap

Posted 2021-10-20 COCO_PEAK_NOODLE

我们知道移除一个子图时最重要的是如何把约束关系移除，我们看看作者的思路，难道他写错了？

  我们知道每一个子图中有固定数量的node_id，INTRA关系下不包含submap_id,则应该是除了自己本身以外的所有node_id
  std::set<NodeId> nodes_to_retain;
  for (const Constraint& constraint : parent_->data_.constraints) {
    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
        constraint.submap_id != submap_id) {
      nodes_to_retain.insert(constraint.node_id);
    }
  }

  // Remove all 'data_.constraints' related to 'submap_id'.
  //LOG(INFO) << "Peak.ding TrimSubmap 1";
  这里借用nodes_to_retain求出submap包含的node_id?为啥要这样求，直接submap_data.node_ids不更直接吗？所以感觉这里作者的写法有问题，原作者的思路应该是不剔除submap_id中的node_id与其他submap之间的联系
  std::set<NodeId> nodes_to_remove;
  {
    std::vector<Constraint> constraints;
    for (const Constraint& constraint : parent_->data_.constraints) {
      if (constraint.submap_id == submap_id) {
        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);
          //LOG(INFO) << "remove node_id " << constraint.node_id;
        }
      } else {
        constraints.push_back(constraint);
      }
    }
    parent_->data_.constraints = std::move(constraints);
  }

  //LOG(INFO) << "Peak.ding nodes_to_remove size " << nodes_to_remove.size();

  // Remove all 'data_.constraints' related to 'nodes_to_remove'.
  //LOG(INFO) << "Peak.ding TrimSubmap 2";
  {
  //按照这个剔除写法，有关submap的所有约束都将剔除
    std::vector<Constraint> constraints;
    for (const Constraint& constraint : parent_->data_.constraints) {
      if (nodes_to_remove.count(constraint.node_id) == 0) {
        constraints.push_back(constraint);
      }
    }
    parent_->data_.constraints = std::move(constraints);
  }