概述
因子图是适用于复杂的估计问题建模的图模型，例如SLAM、SFM问题。因子图和贝叶斯网络（有向无环图）不同，因子图是一种由连接到变量的因子组成的二分图。变量表示估计问题中的未知随机变量，而因子表示从测量或先验知识派生的这些变量的概率约束。
一.因子图
上图是一个三个步长的隐马尔可夫模型（HMM）的贝叶斯网络。在贝叶斯网络中，每个节点都与条件概率密度相关联：马尔科夫链编码先验P(X1)和条件概率P(X2|X1)和P(X3|X2)，观测Zt仅取决于状态Xt，依据条件概率建模为P(Zt|Xt)。给定已知的观测z1，z2，z3，可以最大化后验概率P(X1,X2,X3|Z1=z1,Z2=z2,Z3=z3),进而得到最有可能的状态变量序列(X1,X2,X3)，观Z1，Z2，Z3已知，后验概率与六个因子的乘积有关，其中三个因子来源于马尔科夫链，另外三个似然因子被定义为：
下图展示了一个因子图：
根据如下公式最大化后验概率：
注：我的理解是，上式的实际意义就是在那个x上最有可能看到观测z。
二、机器人运动建模
1.运动建模
上图展示了一个基于马尔科夫链的因子图，其中x1,x2,x3为机器人的位姿变量，黑点表示因子，o1,o2，为里程计的测量值。（这是机器人的运动方程，激光slam中通常使用IMU作为运动方程的测量）。
2.创建一个简单的因子图
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
它会返回一个：
Factor Graph:
size: 3
Factor 0: PriorFactor on 1
prior mean: (0, 0, 0)
noise model: diagonal sigmas [0.3; 0.3; 0.1];
Factor 1: BetweenFactor(1,2)
measured: (2, 0, 0)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
Factor 2: BetweenFactor(2,3)
measured: (2, 0, 0)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
3.省略
4.GTSAM中的非线性优化
在gtsam中使用NonlinearFactorGraph储存因子，使用Values储存变量：
// create (deliberately inaccurate) initial estimate
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));
// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
上面的代码将返回初始位姿，和经过优化后的位姿：
Initial Estimate:
Values with 3 values:
Value 1: (0.5, 0, 0.2)
Value 2: (2.3, 0.1, -0.2)
Value 3: (4.1, 0.1, 0.1)
Final Result:
Values with 3 values:
Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
Value 2: (2, 7.4e-18, -2.5e-18)
Value 3: (4, -1.8e-18, -3.1e-18)