cartographer自動更新地圖 初探

其實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的問題，是以後續是否還有其他問題，後面再分解。