SLAM 关键帧提取与因子图优化函数解读 - void saveKeyFramesAndFactor() 函数详解

此函数实现了关键帧的提取和因子图优化。具体流程如下：

1. 通过比较当前帧与前一帧的位姿变换大小，判断是否为关键帧。

2. 将激光里程计因子、GPS因子和闭环因子添加到因子图中。

3. 执行因子图优化。

4. 将优化后的当前帧位姿加入关键帧队列中，并更新ESKF状态量和方差。

5. 保存当前帧的特征点降采样集合，并更新可视化路径。

总的来说，此函数实现了SLAM中的一个重要步骤，即关键帧提取和因子图优化，通过优化得到更准确的位姿估计，从而提高整个SLAM系统的精度。