LIO-SAM:闭环线程

前言：

在LIO-SAM中后端采用了因子图优化的方法，其中回环检测是单独放在了一个线程之中，需要注意的是此处回环检测线程只进行了回环检测操作并没有进行回环矫正，只是构造了一个闭环因子，加入到因子图中，然后在因子图优化中进行更新位姿。

闭环线程流程：

1.在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧。

这里是对关键帧进行的操作，搜索方式采用的KD-Tree的方式，时间上超过30s。

2. 提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样，组成局部地图map。

3. 执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿。

这里的icp方法是使用pcl库进行icp操作，使用的是迭代最近邻点的匹配方法。如果结果未收敛或者匹配效果比较差吗，则放弃。

向gtsam中添加帧间约束的因子

这里参考上一篇文章中的闭环因子：SLAM基础-因子图优化和gtsam应用_gtsam因子图变量-优快云博客

其实就是估计出当前关键帧和闭环关键帧之间的位姿变换，作为一个帧间约束加入到因子图优化中。