Cartographer之基于运动阈值筛选约束的方法

任务动机：合理地筛选运动约束，提高cartographer后端优化的实时性。

任务描述：针对传统cartographer后端对nodes和submaps之间的约束关系筛选不均衡的情况，提出一种基于运动阈值筛选约束的方法。该方法可以筛除重复和无效的约束，使得有效的约束插入到优化队列进行位姿优化，进而提高Cartographer建图的实时性。

1. Cartographer回环检测和后端优化

在前端匹配完成后，调用InsertToSubmap()函数，将scan插入匹配完成的submap。然后调用pose graph进行回环检测和后端优化。

回环检测的逻辑是每一个新生成的node都会和已完成的submap尝试构建约束。同时，每一个新生成的submap也会和所有的node尝试构建约束。每一个约束会关联对应的node位姿和submap位姿，然后利用最小二乘进行优化求解准确的node位姿和submap位姿。

2. 基于运动阈值筛选约束

建图过程中，后端优化时间的长短主要取决于位姿图优化队列中的运动约束的个数，nodes和submaps之间的运动约束包含两种：

(1)新生成的node和所有已完成的submaps进行构建约束；

(2)新生成的submap和所有历史的nodes来构建约束。