实现原理：

  • 在纯定位时，检测当前Trajectory和之前的Trajectory的匹配状态（约束的匹配情况），对于变化大的子图，在其区域添加新的子图，对于完全变化的区域，移除其子图。
  • 在前端可以写个定时保存功能；
  • 对于原地图和添加子图部分，我选择了对2者进行合并，合并的方式就是先找到原先地图最后submap的index，然后在后面把新增的submap加上去。

定义几个重要变量

  struct ConstraintData{
     int total_num;/*whole data try to match*/
     int valid_num/*valid match*/;
  };
  
  //用于显示当前导航时，地图与原有地图的匹配情况
  MapById<SubmapId, ConstraintData> constraints_in_nav_submaps;
  //MapById<SubmapId, int> all_constraints_in_submap;
  //MapById<SubmapId, ConstraintData> constraints_in_map_submaps;
  //patch submap for environment change; 
  //用于保存与原有地图变化大的子图
  MapById<SubmapId, ConstraintData> constraints_in_patch_submaps;
  //around patch submap for environment change;
  //这个变量很重要，保存的是constraints_in_patch_submaps左右两边的子图，为什么？它的作用是把constraints_in_patch_submaps通过约束固定住，并纠正回来。
  MapById<SubmapId, ConstraintData> constraints_around_patch_submaps;

环境变化之前
在这里插入图片描述
对变化区域添加子图，状态截图
在这里插入图片描述

对于单台车，这样一边运行，一边对动态环境进行子图优化

2022-11-16更新：代码已上传update_map分支，有bug，先下架

2022-11-23更新：连续测试一周，暂时ok，运行中误差不超过2cm，可以自动纠正，代码已上传update_map分支