SLAM基础-因子图优化和gtsam应用

Jiqiang_z

已于 2024-05-29 15:03:36 修改

阅读量6.2k

点赞数 48

分类专栏： LOAM系列阅读笔记 SLAM学习笔记文章标签：算法机器人笔记

于 2024-03-15 20:35:14 首次发布

本文链接：https://blog.csdn.net/jjjqqq123321/article/details/136747255

版权

本文详细介绍了LIO-SAM中如何使用GTSAM进行因子图优化，涉及先验因子、预积分因子和帧间约束因子的构建，以及ISAM2优化器的使用。着重展示了IMU预积分阶段和后端优化中GTSAM的应用过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言：

因子图优化和BA优化、位姿图优化一样，其本质都是解决非线性优化的问题。如果只有路标和位姿之间的因子，和BA优化完全一样。不过因子图是个大筐，什么约束都能加，IMU，轮速计，GPS。

在当前估计点处求导展开转换成线性矩阵用GN方法或者LM求解。

因子图优化常用的库有GTSAM，使用GTSAM解决因子图优化问题的经典SLAM代码有LIO-SAM。

关于因子图优化的讲解可以看深蓝学院的视频公开课：
因子图的理论基础与在机器人中的应用 - 深蓝学院 - 专注人工智能与自动驾驶的学习平台

视频公开课的笔记可参考以下文章：

因子图优化原理（iSAM、iSAM2论文解析）-CSDN博客

下面来分析一下LIO-SAM中如何用GTSAM来解决优化问题。LIO-SAM中一共用到了两个因子图优化器，一个是IMU预积分阶段进行的因子图优化，另外一个是后端阶段进行的因子图优化。

IMU预积分阶段-因子图优化：

LIO-SAM中的因子图如下图所示：

先来看一下LIO-SAM中使用了哪些类型的gtsam因子：

// 1.先验因子 3个参数（变量节点，先验值，协方差矩阵）
gtsam::PriorFactor

// 2.预积分因子 6个参数
//（上一时刻位姿节点，上一时刻速度节点,当前时刻位姿节点，当前时刻速度节点，上一时刻偏置节点，预计分量）
gtsam::ImuFactor

// 3.Between因子 4个参数（上一时刻变量节点，当前时刻变量节点，节点之间的约束，协方差矩阵）
gtsam::BetweenFactor

因子图中的因子类型上主要分为两类，一种是先验因子，一种是帧间约束因子。

1.定义因子图变量

gtsam中 ISAM2因子图优化器、因子图因子对象、因子图状态变量对象的定义

    // ISAM2因子图优化器,ISAM2 是一种增量式因子图优化算法
    gtsam::ISAM2 optimizer;
    // 因子图的因子（约束）对象，该对象用于存储构建的因子。
    gtsam::NonlinearFactorGraph graphFactors;
    // 因子图的状态变量对象（变量的估计值）。
    gtsam::Values graphValues;

2.重置因子图优化器

重置 ISAM2因子图优化器、因子图因子对象、因子图状态变量对象（在第一帧以及每过100帧重置一次）

    /**
     * 重置 ISAM2因子图优化器、因子图因子对象、因子图状态变量对象
     */
    void resetOptimization()
    {
        // Step 1 : 重置ISAM2优化器
        //ISAM2 优化器的参数配置对象
        gtsam::ISAM2Params optParameters;
        // 因子图中每次变化超过该阈值时，ISAM2 优化器将重新线性化非线性因子。
        optParameters.relinearizeThreshold = 0.1;
        // relinearizeSkip 设置为 1，表示每个因子添加到图中后都进行重新线性化。
        optParameters.relinearizeSkip = 1;
        // 使用optParameters对ISAM2优化器对象进行初始化。
        optimizer = gtsam::ISAM2(optParameters);

        // Step 2 : 重置因子