cartographer实现对动态环境的子图优化（对变化大的区域添加新的子图，移除完全变化的子图）

实现原理：

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

定义几个重要变量

  struct ConstraintData{
   
     int total_num;/*whole data try to match*/
     int valid_num/*valid match*/;