因子图的功能

在slam的后端优化问题中，通常会通过⼀些传感器的观测，比如：

• 视觉特征点
• IMU预积分量
• Lidar面点和边缘点（角点）的约束

去构建一个优化问题，求解状态量（如位姿、速度等）。

这个时候存在一个问题：
当给这个系统新增⼀个约束时，就会重新建立所有的约束对状态量的优化问题进行求解。当优化模型增大时，显然进行一次优化的时间也会增加很多；一方面实时性遭遇了挑战，另一方面，很久之前的状态也没有继续更新的必要。

因子图可以解决这个问题：
为了解决这个问题，⼀种方式是使用滑动窗口来控制优化问题的规模，而滑动窗口需要处理好边缘化的问题；另一种方式，可以使用因子图的模型来解决这个问题。

因子图的内部实现：
Kaess等科研人员提出iSAM，即增量平滑和建图，使其可以自动增量处理大规模优化问题；具体来说，其内部使用一种基于概率的贝叶斯树，使得每次给因子图增加一个约束时，会根据贝叶斯树的连接关系，调整和当前结点“关系比较密切”的结点（概率比较大）；这样既保障了优化问题的求解精度，也使得耗时不会随着优化问题的增大而增大。关于因子图优化理论可以参考iSAM，iSAM2相关论文。

因子图中的"边"和"顶点"

• 变量结点：类似g2O中的顶点或者ceres中的参数块，代表需要被优化的变量。（在slam问题中可以是：位姿、速度、IMU零偏这些变量）
• 因子结点：类似g2O中的边或者ceres中的cost function（代价函数），代表约束，如预积分约束、位姿先验约束、帧间位姿约束等

GTSAM关于IMU预积分相关接口

GTSAM库介绍

GTSAM全称是 Georgia Tech Smoothing and Mapping library，是佐治亚理工学院的科研人员基于因子图和贝叶斯网络推出的⼀个C++库文件，如果想在工程中使用因子图优化的相关算法，最常用的方式就是借助GTSAM这个库来实现，因为其内部已经封装了关于因子图优化以及iSAM相关的算法实现，只需要像调用其他第三方库的方式（如openCV，PCL等）调用GTSAM库即可。 关于GTSAM库的详细介绍可以参考其官方文档。

GTSAM官方文档

GTSAM预积分接口

由上面的整体框架，可以看出 LIO-SAM中的IMU预积分节点，该节点最后一个工作，需要前面3个工作完，得到激光雷达里程计后，再进行IMU预积分

• 订阅数据：
  imu原始数据
  激光雷达里程计（来自建图优化节点）
• 功能：
  因子图优化（IMU预积分因子、雷达里程计因子）
  估计IMU零偏
• 发布消息：
  IMU里程计信息

所以IMU预积分要完成的事情就是：
对两个雷达关键帧之间的若干帧IMU进行预积分，以形成预积分约束，对两帧之间的位置、速度、姿态以及零偏进行约束。
GTSAM从4.0版本开始就在内部增加了IMU预积分相关的接口。

下面总结下 GTSAM中的一些相关接口：

预积分相关参数：

gtsam::PreintegrationParams
1

对IMU数据进行预积分之前需要事先知道IMU的噪声，重力方向（与建模的方式有关，z轴的朝向）等参数。

对应LIO-SAM中的代码有如下部分：

boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
1

其中MakeSharedU就是设置重力方向的，其中的设置和IMU的坐标轴建模方向有关

• MakeSharedU 是ENU，就是东北天，内部把imuGravity的值进行了取负。
• MakeSharedD 是NED，就是北东地，内部不改变变量的正负号。

imuGravity是在参数服务器中设置的参数

nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511);
1

        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
1
2

角速度计和陀螺仪的噪声配置

p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2);
1

进行预积分时 ，速度积分得到位置时的噪声

跟预积分相关的计算在这个类中实现：

gtsam::PreintegratedImuMeasurements
1

对应LIO-SAM中的代码有如下部分：

        imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 
        imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 
1
2

这里两个预积分量一个是用来预积分优化，一个用来推算最新位姿

这个类有⼀些重要的接口。
预积分量复位与设置零偏：

resetIntegrationAndSetBias
1

功能：将预积分量复位，也就是清空之前的预积分量，重新开始一个新的预积分量。预积分的计算依赖一个初始的IMU零偏，因此在复位之后需要输入零偏值，这里的复位和重设零偏在一个接口里。
对应LIO-SAM中的代码有如下部分：

            imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
1
2

输入IMU的测量值：

integrateMeasurement
1

输入IMU的测量值，内部会自动实现预积分量的更新以及协方差矩阵的更新

也就是这部分

对应LIO-SAM中的代码有如下部分：

                imuIntegratorOpt_->integrateMeasurement(
                        gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
1
2
3

预积分量的时间长度：

deltaTij
1

第i帧和第j帧的时间差

imuIntegratorOpt_->deltaTij()
1

预积分预测：

predict
1

预积分量可以计算出两帧之间的相对位置、速度、姿态的变化量，根据预积分结果，结合上一帧的状态量就可以计算出下一关键帧的预测

gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
1

根据上一时刻状态，结合预积分结果，对当前状态进行预测