尧图网页设计 ：GTSAM因子图优化与闭环检测代码全解)
LIO-SAM-DetailedNote源码解析(五)GTSAM因子图优化与闭环检测代码全解【免费下载链接】LIO-SAM-DetailedNoteLIO-SAM源码详细注释3D SLAM融合激光、IMU、GPS项目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNoteLIO-SAM-DetailedNote 是一个带详细中文注释的 LIO-SAM 3D SLAM 项目融合激光雷达、IMU、GPS 三类传感器实现高精定位。本篇作为源码系列第五篇完整拆解 LIO-SAM 最核心的两块代码基于 GTSAM 的因子图优化ISAM2 增量求解、里程计/GPS/闭环三类因子的构建与闭环检测候选帧搜索 ICP 匹配。读完你能看懂整个系统如何把漂移的里程计轨迹拉回正轨。一、因子图优化在 LIO-SAM 中的位置LIO-SAM 的全部逻辑只有 4 个 cpp 文件因子图优化与闭环检测都集中在MapOptimization节点中实现见 src/mapOptmization.cpp注意原项目文件名就是 Optmization 拼写。四个节点的协作关系可参考 ROS 节点拓扑图整个数据流可以概括为一句话scan-to-map 给出当前帧位姿初值 → 封装成因子加入 GTSAM 因子图 → ISAM2 增量优化修正所有历史关键帧位姿闭环检测在后台线程周期性执行发现闭环后把闭环边也变成因子加入图中。二、先看懂 LIO-SAM 的因子图长什么样LIO-SAM 的因子图非常简洁每个关键帧位姿是一个Pose3变量节点6 自由度3 平移 3 旋转节点之间用不同的因子连边因子类型GTSAM 类含义噪声水平先验因子PriorFactorPose3第一帧位姿固定为原点附近较松平移 1e-2里程计因子BetweenFactorPose3相邻两关键帧的相对位姿观测很紧1e-6 级GPS 因子GPSFactor当前关键帧的绝对位置约束按 GPS 协方差下限 1m²闭环因子BetweenFactorPose3时间上相距很远的两帧之间的位姿变换由 ICP 拟合得分决定变量节点与因子的声明集中在 mapOptmization.cppNonlinearFactorGraph gtSAMgraph存图ISAM2 *isam是增量优化器。三、ISAM2 增量优化器为什么选它在构造函数中可以看到 ISAM2 的两个关键参数mapOptmization.cppISAM2Params parameters; parameters.relinearizeThreshold 0.1; // 解变化超过0.1就重新线性化 parameters.relinearizeSkip 1; // 每隔1次更新重线性化 isam new ISAM2(parameters);与一次性求解整张图不同ISAM2 是增量式每加入一个新关键帧只把新增的因子喂给它旧图结构Cholesky 分解得以复用所以 LIO-SAM 能实时跑图优化。闭环修正后旧帧位姿变化大relinearizeThreshold 0.1保证解变化超过阈值时自动重新线性化兼顾速度与精度。 补充IMU 预积分模块 src/imuPreintegration.cpp 内部也有一张小 GTSAM 图状态为位姿速度IMU 偏置并且每 100 帧激光里程计就重置一次优化器、用边缘化协方差做先验防止图无限膨胀——这是工程上保持优化效率的常用技巧。四、关键帧策略哪些帧会进入因子图并不是每帧激光都进图。saveFrame()mapOptmization.cpp计算当前帧与上一关键帧的位姿增量只有满足其一才保存为关键帧旋转增量roll / pitch / yaw 任一超过surroundingkeyframeAddingAngleThreshold默认 0.2 rad即约 11.5°平移距离超过surroundingkeyframeAddingDistThreshold默认 1 m。阈值在 config/params.yaml 中配置。这既控制了图的规模也避免了大量冗余节点。五、三大因子详解addOdomFactor / addGPSFactor / addLoopFactor5.1 里程计因子addOdomFactor逻辑在 mapOptmization.cpp分两种情况第一帧添加PriorFactorPose3先验因子噪声对角阵为(1e-2, 1e-2, π², 1e8, 1e8, 1e8)——平移和朝向很松而 roll/pitch 的 1e8 意味着首帧水平姿态几乎被锁死为整个坐标系定基调。后续帧添加BetweenFactorPose3观测值是上一关键帧 → 当前帧的相对位姿噪声为(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)。注意平移噪声1e-6远小于旋转噪声1e-4因为 LIO-SAM 的 scan-to-map 对平移估计最可靠。5.2 GPS 因子addGPSFactorGPS 因子是全局锚点防止长距离累积漂移但有严格的筛选条件mapOptmization.cpp首尾关键帧距离不足 5 m 时不添加刚起步没必要当前帧位姿协方差已经很小poseCovThreshold以下时不添加——图优化自身已经足够准就别让 GPS 拉偏了只取当前帧时刻 ±0.2 s 窗口内的 GPS 里程计GPS 自身协方差超过gpsCovThreshold的无效与上一个 GPS 因子距离不足 5 m 时跳过每隔 5 m 才加一个 GPS 因子。关于高度若useGpsElevation为 false默认则 z 直接取激光里程计的值并设很小噪声即GPS 只约束平面位置不管高度——因为 GPS 高度噪声大。5.3 闭环因子addLoopFactor闭环因子最简单也最值钱把闭环线程塞进loopIndexQueue / loopPoseQueue / loopNoiseQueue三个队列的数据逐条取出构建BetweenFactorPose3加入图中mapOptmization.cpp。它的噪声大小不是拍脑袋定的而是来自 ICP 拟合得分见第七节。六、优化执行saveKeyFramesAndFactor 的完整流程saveKeyFramesAndFactor()mapOptmization.cpp是每帧主流程的收口步骤如下saveFrame()判断是否为关键帧不是则直接返回依次调用addOdomFactor()、addGPSFactor()、addLoopFactor()把新因子挂到gtSAMgraph上调用isam-update(gtSAMgraph, initialEstimate)增量优化若本轮有因子被标记为aLoopIsClosed加了 GPS 或闭环因子则额外连续update()五次让所有受影响的旧关键帧位姿充分收敛——这是闭环后整条轨迹一起动的由来清空图gtSAMgraph.resize(0)从isamCurrentEstimate取回所有关键帧的新位姿用isam-marginalCovariance()获取当前帧协方差更新路径、发布里程计。七、闭环检测从候选搜索到 ICP 匹配闭环在独立线程loopClosureThread()mapOptmization.cpp中按loopClosureFrequency默认 1 Hz运行核心是performLoopClosure()mapOptmization.cpp第 1 步找候选闭环帧detectLoopClosureDistance用 KD-Tree 在全部历史关键帧中做半径搜索historyKeyframeSearchRadius默认 1 m找出空间位置上离我很近的关键帧再从中挑选**时间差超过historyKeyframeSearchTimeDiff默认 20 s**的那一帧作为候选——空间近而时间远才可能是闭环mapOptmization.cpp。第 2 步提取特征并降采样用loopFindNearKeyframes()提取当前关键帧1 帧与候选闭环帧前后相邻若干帧historyKeyframeSearchNum默认 15 帧的角点面点特征并降采样当前帧特征不足 300 点或局部 map 不足 1000 点则放弃。第 3 步ICP 配准并验证icp.align()执行 PCL 的 ICP最大迭代 100 次必须同时满足hasConverged()为真且getFitnessScore() historyKeyframeFitnessScore默认 0.4否则丢弃闭环边的噪声用fitnessScore填充 6 维对角阵——配准越准闭环约束越紧这是一个很优雅的自适应设计。第 4 步不直接改位姿注意代码注释特别强调闭环时不立即更新当前帧位姿而是把闭环边数据推入队列等下一帧saveKeyFramesAndFactor()执行时作为因子进图由图优化统一修正全局轨迹。这样保证了轨迹的数学一致性。闭环成功后RViz 中会看到闭环点青球与闭环边黄色连线效果如下官方文档目录中还有闭环效果演示动画可以观察轨迹回环收敛的过程八、关键参数速查与小结参数默认值作用surroundingkeyframeAddingDistThreshold1.0 m关键帧最小平移间隔surroundingkeyframeAddingAngleThreshold0.2 rad关键帧最小旋转间隔historyKeyframeSearchRadius1.0 m闭环候选空间搜索半径historyKeyframeSearchTimeDiff20.0 s闭环候选最小时间差historyKeyframeFitnessScore0.4ICP 闭环接受阈值gpsCovThreshold0.5GPS 协方差上限imuRPYWeight0.1IMU 姿态对 roll/pitch 的融合权重一句话小结LIO-SAM 的高精度来自紧的里程计因子维持局部一致性 GPS/闭环因子提供全局约束ISAM2 的增量求解让它实时可行闭环检测则用空间近、时间远 ICP 双重验证的朴素而稳健策略找到回环再把配准质量转化为因子噪声——这套工程设计对理解任何激光 SLAM 系统都是很好的范本。本仓库每个源码文件都有详细中文注释配合 README.md 的整体流程说明与 frames.pdf 的坐标系定义文档可以逐行对照学习。【免费下载链接】LIO-SAM-DetailedNoteLIO-SAM源码详细注释3D SLAM融合激光、IMU、GPS项目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNote创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 LIO-SAM-DetailedNote源码解析(五)：GTSAM因子图优化与闭环检测代码全解
LIO-SAM-DetailedNote源码解析(五)GTSAM因子图优化与闭环检测代码全解【免费下载链接】LIO-SAM-DetailedNoteLIO-SAM源码详细注释3D SLAM融合激光、IMU、GPS项目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNoteLIO-SAM-DetailedNote 是一个带详细中文注释的 LIO-SAM 3D SLAM 项目融合激光雷达、IMU、GPS 三类传感器实现高精定位。本篇作为源码系列第五篇完整拆解 LIO-SAM 最核心的两块代码基于 GTSAM 的因子图优化ISAM2 增量求解、里程计/GPS/闭环三类因子的构建与闭环检测候选帧搜索 ICP 匹配。读完你能看懂整个系统如何把漂移的里程计轨迹拉回正轨。一、因子图优化在 LIO-SAM 中的位置LIO-SAM 的全部逻辑只有 4 个 cpp 文件因子图优化与闭环检测都集中在MapOptimization节点中实现见 src/mapOptmization.cpp注意原项目文件名就是 Optmization 拼写。四个节点的协作关系可参考 ROS 节点拓扑图整个数据流可以概括为一句话scan-to-map 给出当前帧位姿初值 → 封装成因子加入 GTSAM 因子图 → ISAM2 增量优化修正所有历史关键帧位姿闭环检测在后台线程周期性执行发现闭环后把闭环边也变成因子加入图中。二、先看懂 LIO-SAM 的因子图长什么样LIO-SAM 的因子图非常简洁每个关键帧位姿是一个Pose3变量节点6 自由度3 平移 3 旋转节点之间用不同的因子连边因子类型GTSAM 类含义噪声水平先验因子PriorFactorPose3第一帧位姿固定为原点附近较松平移 1e-2里程计因子BetweenFactorPose3相邻两关键帧的相对位姿观测很紧1e-6 级GPS 因子GPSFactor当前关键帧的绝对位置约束按 GPS 协方差下限 1m²闭环因子BetweenFactorPose3时间上相距很远的两帧之间的位姿变换由 ICP 拟合得分决定变量节点与因子的声明集中在 mapOptmization.cppNonlinearFactorGraph gtSAMgraph存图ISAM2 *isam是增量优化器。三、ISAM2 增量优化器为什么选它在构造函数中可以看到 ISAM2 的两个关键参数mapOptmization.cppISAM2Params parameters; parameters.relinearizeThreshold 0.1; // 解变化超过0.1就重新线性化 parameters.relinearizeSkip 1; // 每隔1次更新重线性化 isam new ISAM2(parameters);与一次性求解整张图不同ISAM2 是增量式每加入一个新关键帧只把新增的因子喂给它旧图结构Cholesky 分解得以复用所以 LIO-SAM 能实时跑图优化。闭环修正后旧帧位姿变化大relinearizeThreshold 0.1保证解变化超过阈值时自动重新线性化兼顾速度与精度。 补充IMU 预积分模块 src/imuPreintegration.cpp 内部也有一张小 GTSAM 图状态为位姿速度IMU 偏置并且每 100 帧激光里程计就重置一次优化器、用边缘化协方差做先验防止图无限膨胀——这是工程上保持优化效率的常用技巧。四、关键帧策略哪些帧会进入因子图并不是每帧激光都进图。saveFrame()mapOptmization.cpp计算当前帧与上一关键帧的位姿增量只有满足其一才保存为关键帧旋转增量roll / pitch / yaw 任一超过surroundingkeyframeAddingAngleThreshold默认 0.2 rad即约 11.5°平移距离超过surroundingkeyframeAddingDistThreshold默认 1 m。阈值在 config/params.yaml 中配置。这既控制了图的规模也避免了大量冗余节点。五、三大因子详解addOdomFactor / addGPSFactor / addLoopFactor5.1 里程计因子addOdomFactor逻辑在 mapOptmization.cpp分两种情况第一帧添加PriorFactorPose3先验因子噪声对角阵为(1e-2, 1e-2, π², 1e8, 1e8, 1e8)——平移和朝向很松而 roll/pitch 的 1e8 意味着首帧水平姿态几乎被锁死为整个坐标系定基调。后续帧添加BetweenFactorPose3观测值是上一关键帧 → 当前帧的相对位姿噪声为(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)。注意平移噪声1e-6远小于旋转噪声1e-4因为 LIO-SAM 的 scan-to-map 对平移估计最可靠。5.2 GPS 因子addGPSFactorGPS 因子是全局锚点防止长距离累积漂移但有严格的筛选条件mapOptmization.cpp首尾关键帧距离不足 5 m 时不添加刚起步没必要当前帧位姿协方差已经很小poseCovThreshold以下时不添加——图优化自身已经足够准就别让 GPS 拉偏了只取当前帧时刻 ±0.2 s 窗口内的 GPS 里程计GPS 自身协方差超过gpsCovThreshold的无效与上一个 GPS 因子距离不足 5 m 时跳过每隔 5 m 才加一个 GPS 因子。关于高度若useGpsElevation为 false默认则 z 直接取激光里程计的值并设很小噪声即GPS 只约束平面位置不管高度——因为 GPS 高度噪声大。5.3 闭环因子addLoopFactor闭环因子最简单也最值钱把闭环线程塞进loopIndexQueue / loopPoseQueue / loopNoiseQueue三个队列的数据逐条取出构建BetweenFactorPose3加入图中mapOptmization.cpp。它的噪声大小不是拍脑袋定的而是来自 ICP 拟合得分见第七节。六、优化执行saveKeyFramesAndFactor 的完整流程saveKeyFramesAndFactor()mapOptmization.cpp是每帧主流程的收口步骤如下saveFrame()判断是否为关键帧不是则直接返回依次调用addOdomFactor()、addGPSFactor()、addLoopFactor()把新因子挂到gtSAMgraph上调用isam-update(gtSAMgraph, initialEstimate)增量优化若本轮有因子被标记为aLoopIsClosed加了 GPS 或闭环因子则额外连续update()五次让所有受影响的旧关键帧位姿充分收敛——这是闭环后整条轨迹一起动的由来清空图gtSAMgraph.resize(0)从isamCurrentEstimate取回所有关键帧的新位姿用isam-marginalCovariance()获取当前帧协方差更新路径、发布里程计。七、闭环检测从候选搜索到 ICP 匹配闭环在独立线程loopClosureThread()mapOptmization.cpp中按loopClosureFrequency默认 1 Hz运行核心是performLoopClosure()mapOptmization.cpp第 1 步找候选闭环帧detectLoopClosureDistance用 KD-Tree 在全部历史关键帧中做半径搜索historyKeyframeSearchRadius默认 1 m找出空间位置上离我很近的关键帧再从中挑选**时间差超过historyKeyframeSearchTimeDiff默认 20 s**的那一帧作为候选——空间近而时间远才可能是闭环mapOptmization.cpp。第 2 步提取特征并降采样用loopFindNearKeyframes()提取当前关键帧1 帧与候选闭环帧前后相邻若干帧historyKeyframeSearchNum默认 15 帧的角点面点特征并降采样当前帧特征不足 300 点或局部 map 不足 1000 点则放弃。第 3 步ICP 配准并验证icp.align()执行 PCL 的 ICP最大迭代 100 次必须同时满足hasConverged()为真且getFitnessScore() historyKeyframeFitnessScore默认 0.4否则丢弃闭环边的噪声用fitnessScore填充 6 维对角阵——配准越准闭环约束越紧这是一个很优雅的自适应设计。第 4 步不直接改位姿注意代码注释特别强调闭环时不立即更新当前帧位姿而是把闭环边数据推入队列等下一帧saveKeyFramesAndFactor()执行时作为因子进图由图优化统一修正全局轨迹。这样保证了轨迹的数学一致性。闭环成功后RViz 中会看到闭环点青球与闭环边黄色连线效果如下官方文档目录中还有闭环效果演示动画可以观察轨迹回环收敛的过程八、关键参数速查与小结参数默认值作用surroundingkeyframeAddingDistThreshold1.0 m关键帧最小平移间隔surroundingkeyframeAddingAngleThreshold0.2 rad关键帧最小旋转间隔historyKeyframeSearchRadius1.0 m闭环候选空间搜索半径historyKeyframeSearchTimeDiff20.0 s闭环候选最小时间差historyKeyframeFitnessScore0.4ICP 闭环接受阈值gpsCovThreshold0.5GPS 协方差上限imuRPYWeight0.1IMU 姿态对 roll/pitch 的融合权重一句话小结LIO-SAM 的高精度来自紧的里程计因子维持局部一致性 GPS/闭环因子提供全局约束ISAM2 的增量求解让它实时可行闭环检测则用空间近、时间远 ICP 双重验证的朴素而稳健策略找到回环再把配准质量转化为因子噪声——这套工程设计对理解任何激光 SLAM 系统都是很好的范本。本仓库每个源码文件都有详细中文注释配合 README.md 的整体流程说明与 frames.pdf 的坐标系定义文档可以逐行对照学习。【免费下载链接】LIO-SAM-DetailedNoteLIO-SAM源码详细注释3D SLAM融合激光、IMU、GPS项目地址: https://gitcode.com/gh_mirrors/li/LIO-SAM-DetailedNote创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考