cartographer导航的时候更新地图

我的方案，
1- 使用谷歌作者的overlapping_submaps_trimmer_2d来缩减地图，但是这个计算量太大了，我使用了局部计算来代替全局计算，这样间隔一段时间就缩减1-2个地图，计算量有所降低，其实还有更低的（还未实现，思路是不重复计算所有地图，维护一个数据，每次只更新新增的部分）

      std::vector<SubmapId> near_submap;
      if(newly_finished_submap_flag)
      {
   
        cartographer::transform::Rigid3d submap_global_pose = 
        GetSubmapDataUnderLock(newly_finished_submap_id