SLAM基础-因子图优化

前言：

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

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

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

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

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

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

下面来分析一下LIO-SAM中如何用GTSAM来解决优化问题，看清楚是谁对谁的优化。

LIO-SAM中对IMU预积分的因子图优化：

imu预积分器定义和初始化

gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;// imu预积分的噪声协方差
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // acc white noise in continuous
p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2);     // gyro white noise in continuous
p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2);          // error committed in integrating position from velocities
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // assume zero initial bias// imu预积分器，用于因子图优化
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization

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

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

重置 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 : 重置因子图因子对象gtsam::NonlinearFactorGraph newGraphFactors;graphFactors = newGraphFactors;// Step 3 : 重置因子图状态变量对象gtsam::Values NewGraphValues;graphValues = NewGraphValues;}

向因子图因子对象中添加先验因子（即约束）

下面同时添加了3种不同状态变量的因子，即位姿和位姿之间的约束、速度和速度之间的约束、imu偏置和imu偏置之间的约束。（下面的举例是以重置后的状态量为例）

在这里设置因子时有三个参数：参数1：待优化变量位姿参数2：激光里程计先验因子（来自后端）参数3：里程计因子置信度（协方差矩阵）

其中参数1：待优化变量位姿是imu里程计结果（在上一帧先验因子到基础上加上imu预积分的结果），所以这里的约束相当于激光里程计先验因子对IMU预积分结果的一个约束。

            // 1.添加里程计位姿先验因子，将lidar数据转化到imu坐标系下prevPose_ = lidarPose.compose(lidar2Imu);// 设置因子（初始位姿和置信度）// note：参数1：待优化变量位姿 参数2：激光里程计先验因子（来自后端） 参数3：里程计因子置信度（协方差矩阵）gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);// 约束加入到因子图graphFactors.add(priorPose);// 2.添加速度先验因子prevVel_ = gtsam::Vector3(0, 0, 0);// 这里设置的先验速度为0，因此在添加因子时，速度的置信度priorVelNoise设置的就差一些。gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);graphFactors.add(priorVel);// 3.添加imu偏置先验因子// 初始化零偏prevBias_ = gtsam::imuBias::ConstantBias();gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);graphFactors.add(priorBias);

向因子图状态变量对象中添加状态变量（即待优化变量）

            // Step 0.2 向因子图状态变量对象中添加状态变量（即待优化变量）// note:以上是添加的约束（约束就是因子）到因子图，下面是添加状态变量// 4.变量节点赋初值graphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);

更新因子图优化器中的因子和状态变量

            // 5.优化一次  // ！：每次优化完成一次后，就会清空因子图和变量，优化器是每100帧重置一次。因此每次向优化器内添加的因子图和变量是一一对应的。optimizer.update(graphFactors, graphValues);optimizer.update();graphFactors.resize(0);graphValues.clear();

因子图优化器优化后的结果

优化变量的结果以及位姿、速度、偏置噪声的更新结果如下所示：

            // 位姿、速度、偏置噪声的更新结果如下gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key - 1)));gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key - 1)));gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key - 1)));// note：优化结果gtsam::Values result = optimizer.calculateEstimate();// 6.利用优化结果更新状态量// 更新当前帧位姿、速度prevPose_ = result.at<gtsam::Pose3>(X(key));prevVel_ = result.at<gtsam::Vector3>(V(key));// 更新当前帧状态prevState_ = gtsam::NavState(prevPose_, prevVel_);// 更新当前帧imu偏置prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));

--------------------------------------------注意因子和先验因子的区别-----------------------------------------