faster lio 回环 加入GTSAM优化的记录

首先感谢这位博主的文章：https://blog.csdn.net/weixin_41281151/article/details/125371285，其中部分代码参考于改博主中的github： https://github.com/kahowang/FAST_LIO_SAM
不同的是，我使用的是faster lio进行更改，还有不少细节上的不同。

这份工作的目的

1. 为了构建一个高精度的点云地图
2. 去除动态物体，提高精度，所以选择以ivox进行搜寻最近点的faster lio

过程

1. 这一步和上述博主的一样，首先引入关键帧的结构，引入gtsam进行优化，加入回环检测，矫正位姿和地图，然后做到效果差不多。

2. but问题来了，（室内正常）在室外过程中，绕了一个周长近一公里的矩形，回到起点，发现此时位置在z轴上高了六七米，随后回环检测回环矫正，通过gtsam矫正发现，随着回环匹配帧插入的数量增加，也就插入五六个关键帧，此时的位置z轴才拉回0。（回环匹配没问题，在第一个回环匹配帧插入后，gtsam优化，z轴的高度为两三米）。but就算后面拉回去了，地图的精度反而下降了，看上去就感觉过优化。如下图所示。z=5到z=6这一段的精度就太差了，反而使得fast lio的精度优势下降了。(用墙的厚度来评估地图质量，z=5处最差)
   

3. 于是我觉得需要降低优化，认为faster lio的框架下的直线行走精度够高，不需要gtsam优化，所以不将“大于设定距离而生成的关键帧A”插入优化，而仅仅插入“大于设定旋转角度而生成的关键帧B”插入优化，在触发回环再引入关键帧即可优化，然后对关键帧B间的关键帧A进行配准（如下图所示）。but结果显示因为gtsam含关键帧太少，反而回环优化后效果还是有点不对，矩形右边还是一样高。
   

4. 对2和3进行调参，分别对里程计的噪声、回环噪声、生成关键帧的条件等进行调节，所生成的地图（矩形下边）依然存在很大的问题。

结论：仅靠一个回环优化难以解决较大的累计误差，而累计误差过大，GTSAM回环优化反而会使地图质量下降。所以应当使用BA矫正，但是得过滤一些，选好优化的点对，否则计算量过大。