将代码生成伪代码只保留括号格式和中文void saveKeyFramesAndFactor 计算当前帧与前一帧位姿变换如果变化太小不设为关键帧反之设为关键帧 if saveFrame == false && gnss_bufferempty return; 激光里程计因子from fast-lio 输入的是frame_relative pose 帧间位姿b

函数 saveKeyFramesAndFactor(): 如果 saveFrame() 返回 false 并且 gnss_buffer 为空，直接返回。 添加激光里程计因子（输入是帧间位姿）。 添加 GPS 因子。 添加闭环因子（基于欧氏距离的检测）。 执行优化，如果有回环因子，多更新几次。 清空保存的因子图。 获取优化结果和当前帧位姿结果。 将当前帧位置加入 cloudKeyPoses3D 队列中。 将当前帧位姿加入 cloudKeyPoses6D 队列中。 获取位姿协方差。 更新 ESKF 状态和方差。 保存当前帧激光平面点，降采样集合。 更新路径可视化。