因子图及GTSAM库介绍

因子图优化及 GTSAM 库介绍

因子图

在 slam 的后端优化问题中，我们通常会通过一些传感器的观测，比如视觉特征点， IMU 预积分量， Lidar面点和边缘点的约束去构建一个优化问题，求解状态量（如位姿、速度等），这个时候我们考虑一个问题，当给这个系统新增一个约束时，我们就会重新对所有的约束的状态的优化问题进行求解，当图优化模型增大时，显然进行一次优化的时间也会增加很多，一方面实时性遭遇了挑战，另一方面，很久之前的状态似乎也没有继续更新的必要。为了解决这个问题，一种方式是使用滑动窗口来控制优化问题的规模，通常来讲滑动窗口需要好处理边缘化的问题，另一方面，我们可以使用因子图的模型来解决这个问题

Kaess 等科研人员提出 iSAM ，即增量平滑和建图，使其可以自动增量处理大规模优化问题，具体来说，其内部使用一种基于概率的贝叶斯树，使得每次给因子图增加一个约束时，其会根据贝叶斯树的连接关系，调整和当前结点 “ 关系比较密切 ” 的结点，比如，t=10时刻的结点加进来，他会优先处理t=9和t=8的节点。如此，既保障了优化问题的求解精度，也使得耗时不会随着优化问题的增大而增大。

滑动窗口一般需要自己去调整滑动，但是iSAM就会自动选择关系密切的节点。

关于因子图优化理论可以参考 iSAM ， iSAM2 相关论文等文献。

因子图中一些概念

变量结点：类似 g2O 中的顶点或者 ceres 中的参数块，代表需要被优化的变量，可能就代表着一个位姿，一个零偏等

因子结点：类似 g2O 中的边或者 ceres 中的 cost function ，代表约束，如预积分约束、位姿先验约束、帧间位姿约束等

GTSAM库的介绍

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

关于 GTSAM 库的详细介绍可以参考其官方文档 GTSAM | GTSAM is a BSD-licensed C++ library that implements sensor fusion for robotics and computer vision using factor graphs.

GTSAM 关于 IMU 预积分相关接口介绍

通过第八章相关介绍可知，我们对两个关键帧之间的若干帧 IMU 进行预积分，以形成预积分约束，对两帧之间的位置、速度、姿态以及零偏进行约束。 GTSAM 从 4.0 版本开始就在内部增加了 IMU 预积分相关的接口，因此，为了实现把预积分因子加入到图优化框架中，我们有必要熟悉一下 GTSAM 中跟 IMU 预积分相关的接口定义。

gtsam::PreintegrationParams

预积分相关参数，我们对 IMU 数据进行预积分之前通常需要事先知道 IMU 的噪声，重力方向等参数

gtsam::PreintegratedImuMeasurements

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

这个类有一些重要的接口

(1) resetIntegrationAndSetBias

将预积分量复位，也就是说清空刚刚的预积分量，重新开始一个新的预积分量，

比如刚刚计算了一个第一帧到第二帧的预积分，然后不想重新创建一个预积分类，复用之前的对象，就需要reset

注意：预积分的计算依赖一个初始的 IMU 零偏，因此，在复位之后需要输入零偏值，所以这里复位和重设零偏在一个接口里。

(2) integrateMeasurement

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

(3) deltaTij

预积分量跨越的时间长度

(4) predict

使用之前的状态和零偏，预测现在的状态。

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

预积分前端代码讲解

该模块涉及到的变量结点

gtsam::Pose3 // 表示六自由度位姿 gtsam::Vector3 // 表示三自由度速度 gtsam::imuBias::ConstantBias //表示 IMU 零偏

以上也是预积分模型中涉及到的三种状态变量

涉及到的因子结点

gtsam::PriorFactor

先验因子，表示对某个状态量 T 的一个先验估计，约束某个状态变量的状态不会离该先验值过远

gtsam::ImuFactor

imu 因子，通过 IMU 预积分量构造出 IMU 因子，即 IMU 约束，相当于一个帧间约束

gtsam::BetweenFactor

状态量间的约束，约束相邻两状态量之间的差值不会距离该约束过远