cartographer自动更新地图 初探

Posted 2021-07-26 Hill_LAI

其实cartographer的地图更新，就是用新扫描的地图替换老地图，这里在加载老地图的时候，要加载TrajectoryState::FROZEN状态的地图，这样新地图就按照老地图为坐标系进行匹配和导航了。
cartographer里面自带的移除老地图的功能是考虑建图模式的情况，所以在导航模式时，我们不能任性移除老地图，因为这样会带来定位灾难（老地图可能因为移除局部而整体变形）
所以我们加载老地图的时候，加载模式为TrajectoryState::FROZEN状态，所以我们只要在结束导航的时候，自己写一个接口实现新老地图的比较然后移除老地图，保存新地图即可。（效果还可以哦）

talk is cheap , show my code

  bool MapBuilder::SerializeStateToFileAfterUpdate(
      bool include_unfinished_submaps, const std::string& filename) {

    //. stop thread to trim submap 
    pose_graph_->SetTrimerFlag(false);

    // trim overlap submap first
    pose_graph_->TrimSubmapIdByManual();

    io::ProtoStreamWriter writer(filename);
    io::UpdateAndWritePbStream(map_trajectory_id_, *pose_graph_, all_trajectory_builder_options_, &writer,
                      include_unfinished_submaps);
    return (writer.Close());
  }

TrimSubmapIdByManual函数，

void PoseGraph2D::TrimSubmapIdByManual() {
  absl::MutexLock locker(&mutex_);

  //加载map的时候把约束关系保存在map_constraint_中，因为是FROZEN加载，
  //所以后端优化用不到这些约束，但是在地图更新时要用到，所以提前保存在map_constraint_中
  data_.constraints.insert(data_.constraints.end(), map_constraint_.begin(),
                           map_constraint_.end());

  TrimmingHandle trimming_handle(this);
  for (auto& trimmer : trimmers_) {
    trimmer->TrimBeforeUpdate(&trimming_handle);
  }

  return;
}

TrimBeforeUpdate函数，就是原来的Trim函数，由于我之前为了解决前端阻塞问题，修改了Trim函数，所以这里重写了一个。

void OverlappingSubmapsTrimmer2D::TrimBeforeUpdate(Trimmable* pose_graph) {
  const auto submap_data = pose_graph->GetOptimizedSubmapData();

  const MapLimits first_submap_map_limits =
      std::static_pointer_cast<const Submap2D>(submap_data.begin()->data.submap)
          ->grid()
          ->limits();
  SubmapCoverageGrid2D coverage_grid(first_submap_map_limits);
  const std::map<SubmapId, common::Time> submap_freshness =
      ComputeSubmapFreshness(submap_data, pose_graph->GetTrajectoryNodes(),
                             pose_graph->GetConstraints());
  const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
      submap_freshness, submap_data, &coverage_grid);
  const std::vector<SubmapId> submap_to_remove = FindSubmapIdsToTrim(
      coverage_grid, all_submap_ids, fresh_submaps_count_,
      min_covered_area_ / common::Pow2(coverage_grid.resolution()));
  current_submap_count_ = submap_data.size() - submap_to_remove.size();
  for (const SubmapId& id : submap_to_remove) {
              pose_graph->TrimSubmap(id);
  }
}

还有一些细节，就是在保存地图的时候，也要适当修改，效果真的很赞！这样slam有了更新功能，是不是不用反复建图了！
因为是初步实现功能，这是一个LongLife的问题，所以后续是否还有其他问题，后面再分解。