概述
GTSAM（Georgia Tech Smoothing and Mapping）是基于因子图的C++库，本篇基于GTSAM对因子图优化做一个简单了解和梳理，并以LIO-SAM为例进一步分析因子图优化在SLAM中的应用。
参考链接：
0\]gtsam官方文档
\[1\]https://blog.csdn.net/QLeelq/article/details/111368277
\[2\]https://zhuanlan.zhihu.com/p/621999120
## 1.基本概念
* 因子图：因子图是一种无向图，由变量和因子组成
* 变量：待优化的状态，可以有多个变量，如机器人位姿、IMU偏置等，每个变量包含了多帧的信息，如位姿`X={x1, x2, ... , xn}`
* 因子：对应图优化中的边, 即两个变量或者观测与变量之间的约束，如两帧之间的位姿变换、3D点到相机图像的投影关系
* 先验因子：基于GPS或者其他模块得到的观测信息或者位姿初值
* 值：指变量的具体数值
如下图所示，`{x1,x2,x3}`为变量
黑色方块为因子，其中连接两个变量的为二元因子，如`o12, o23`;
连接一个变量的为一元因子，如`f1, f2, f3, p1`,其中`p1`为先验因子，`f1, f2, f3`为观测因子
## 2.官方例子说明
### 2.1 机器人运动模型
下图为gtsam官方文档中最简单的一个例子，一个基于马尔可夫链的机器人运动模型，即当前时刻的机器人位姿只由机器人上一时刻位姿决定。
如上图所示：
* x1,x2,x3为变量，表示机器人的位姿
* f0为一元先验因子，可以是初始位姿
* f1,f2为二元因子，表示机器人之间的运动，可以是IMU或激光SLAM的里程计信息
#### 2.1.1建立因子图
对于某个问题，在建立关于该问题的数学模型后，就可以基于数学模型建立因子图。gtsam为不同的数学模型内置了现成的模板，在创建因子图时只需要根据不同的模型选择不同的模板即可。
上述机器人运动模型因子图创建过程如下所示：
```cpp
// step1:构建一个空的非线性因子图
NonlinearFactorGraph graph;
// step2:为因子图添加因子并连接到变量
// [1]构建先验因子，也就是图中的f_0, 这里使用二维姿态（x,y,theta）简化问题
Pose2 priorMean(0.0, 0.0, 0.0);
// [2]初始化高斯噪声，代表我们对该因子的不确定性
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
// [3]将先验因子加入因子图, 其中的1表示该因子连接到第1个变量
graph.add(PriorFactor(1, priorMean, priorNoise));
// [1]构建里程计因子，也就是图中的f_1,f_2, 往前移动2米，y轴不便，theta不变
Pose2 odometry(2.0, 0.0, 0.0);
// [2]初始化高斯噪声
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// [3]将里程计因子加入因子图, (1,2)(2,3)分别代表该里程计约束是从变量1到变量2，从变量2到变量3
graph.add(BetweenFactor(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor(2, 3, odometry, odometryNoise));
// step3:设置各个变量的初始值
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
// step4:调用优化器并使用设定好的初始值对因子图优化
Values result = LevenberMarquardtOptimizer(graph, initial).optimize();
```
### 2.3机器人运动建模
进一步的，为上述运动模型添加观测模型，这里以GPS的测量为例
如上图所示：
* x1,x2,x3为变量，表示机器人的位姿
* o12,o23为二元因子，表示机器人之间的运动
* f1,f2,f3为一元观测因子，即GPS的观测值
#### 2.3.1自定义观测因子
通过`NoiseModelFactor1`模板类定义新的一元因子，其中`T`为一元因子所对应变量的类型。
需要针对新因子定义一下几个信息：
（1）有关传感器观测值的私有成员变量。
（2）有关因子初始化的构造函数。
（3）用于计算评估误差的误差函数，功能包括：返回变量和观测值之间的残差信息、计算用于非线性优化的雅克比矩阵。
**以定义一元GPS因子为例**
（1）定义GPS的测量值`mx_, my_`、
（2）定义GPS因子的构造函数，在使用时通过该函数实例化因子。用给定的`x，y`值初始化`mx_, my_`观测值，并给定与该因子连接的变量代号`j`以及观测的噪声协方差矩阵`model`。
（3）重载用于计算评估误差的函数evaluateError：残差 = 变量值 - 观测值
**公式定义如下：**
**自定义因子代码如下：**
```cpp
// 自定义因子是Pose2类型的，Pose2对应2D位姿的李群变换矩阵，即3x3的一个矩阵，包括2x2的旋转和2x1的平移
class UnaryFactor: public NoiseModelFactor1 {
// 观测的状态包括x和y两个值
double mx_, my_;
public:
// 成员函数， 在参数列表中初始化所关联变量的Id, 噪声模型， 测量值x和y
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1(model, j), mx_(x), my_(y){}
// 残差函数模型，残差 = 变量值 - 观测值
// 输入：当前时刻的位姿q，对误差求导的雅格比矩阵H
// 位姿q=[qx,qy,angle]为三维，误差为两维，因此误差函数对位姿求导的雅格比矩阵为2x3
Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const
{
// 如果存在雅格比矩阵，对其进行更新
if(H)
{
// Jac是误差函数对位姿求导的雅格比矩阵，因为观测函数设置的比较简单，因此雅格比矩阵也很简单，具体的公式推导见下图
gtsam::Matrix Jac = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();;
(*H) = Jac;
}
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
};
```
**创建因子图**
```cpp
// step1:构建一个空的非线性因子图
NonlinearFactorGraph graph;
// step2:为因子图添加因子并连接到变量
// 建立观测的噪声模型
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
// 加入自定义因子
graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise));
// step3:设置各个变量的初始值
Values initial;
initial.insert(1, 0.1, 0.0);
initial.insert(2, 2.0, -0.1);
initial.insert(3, 4.1, 0.1);
// step4:调用优化器并使用设定好的初始值对因子图优化
Values result = LevenberMarquardtOptimizer(graph, initial).optimize();
```
## 3.LIO-SAM中的因子图应用
### 3.1IMU因子
在LIO-SAM的`imageProjection`模块主要对IMU数据进行积分，使用gtsam的IMU预积分模块。该模块订阅雷达里程计的位姿，将该位姿作为初始积分的先验，然后通过gtsam的IMU预积分模块得到IMU高频里程计，同时更新偏置信息，因子图如下：
在这个过程中：
* 变量：位姿(旋转和平移)、速度、偏置
* 先验因子：位姿的先验因子为雷达里程计的位姿，速度和偏置的先验因子为0
* 观测因子：IMU因子，每个时刻IMU的角速度加速度，通过观测方程转化为观测值即位姿
**1.创建因子图并添加先验因子**
```cpp
// 声明非线性因子图
gtsam::NonlinearFactorGraph graphFactors;
// [1]加入雷达里程计先验因子
// 将雷达里程计位姿转换到IMU坐标系作为先验
prevPose_ = lidarPose.compose(lidar2Imu);
// 创建因子，X(0)表示先验因子连接到X第一个变量，prevPose_为先验因子具体的值，priorPoseNoise为之前定义的噪声
gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise);
// 添加因子
graphFactors.add(priorPose);
// [2]加入速度先验因子
prevVel_ = gtsam::Vector3(0, 0, 0);// 初始化为0
gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise);// 创建
graphFactors.add(priorVel);// 加入
// [3]加入Bias先验因子
prevBias_ = gtsam::imuBias::ConstantBias();// 初始化为0
gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise);// 创建
graphFactors.add(priorBias);// 添加
// 将初始状态设置为因子图变量的初始值
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// 使用优化器对变量进行更新
optimizer.update(graphFactors, graphValues);
```
**2.添加IMU因子**
通过gtsam自带的积分器，已经将一段时间内(两帧激光之间)的IMU进行了连续积分，积分角速度得到旋转，积分加速度得到线速度和位姿
```cpp
// 使用IMU预积分的结果构建IMU因子，并加入因子图中
// 将IMU的连续积分结果封装为gtsam::PreintegratedImuMeasurements格式
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast(*imuIntegratorOpt_);
// 从preint_imu中拿出一次积分结果即IMU因子，如上图这个因子连接了相邻两个时刻的位姿、速度和上一时刻的Bias
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);
// imuIntegratorOpt_->predict输入之前时刻的状态和偏差，预测当前时刻的状态
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
// 将IMU预积分的结果作为当前时刻因子图变量的初值
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
// 更新一次优化器
optimizer.update(graphFactors, graphValues);
optimizer.update();
// 从优化器中获取当前经过优化后估计值
gtsam::Values result = optimizer.calculateEstimate();
prevPose_ = result.at(X(key));
prevVel_ = result.at(V(key));
prevState_ = gtsam::NavState(prevPose_, prevVel_);
prevBias_ = result.at(B(key));
```
### 2.地图优化
在LIO-SAM的地图优化模块，只在添加关键帧时进行了因子图优化。因子图如下所示：
* 变量：关键帧位姿
* 因子：激光里程计因子、GPS因子、回环检测因子
下面一段代码是关于添加激光里程计因子的过程，对于第一帧添加的是PriorFactor类型的先验因子，对于后续帧添加的是BetweenFactor类型的二元因子，过程和上述一样。
```cpp
void addOdomFactor()
{
if (cloudKeyPoses3D->points.empty())
{
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
gtSAMgraph.add(PriorFactor(0, trans2gtsamPose(transformTobeMapped), priorNoise));
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
}else{
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
gtSAMgraph.add(BetweenFactor(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
}
}
```
## 总结
综上来看，基于gtsam的因子图优化代码结构是比较清晰的，只不过在使用之前需要根据自己的误差模型选择合适的模板，针对里程计类型、GPS类型等gtsam都提供了相应的模板，对于gtsam库中没有的模板可以自行定义。