GTSAM 因子图：读懂现代 SLAM 后端

1. 满屏 BetweenFactor 从哪读起

第一次打开 LIO-SAM 或 Kimera 后端，变量是 X(i)、V(i)，边是 BetweenFactor、ImuFactor——若不懂因子图，只能对着 grep 结果猜语义，改噪声权重也不知影响哪条边。GTSAM 的好处是 API 与数学几乎一一对应：读懂 NonlinearFactorGraph 和 Values，就等于读懂后端在最小化什么，而不是重新学一套「SLAM 框架名」。后端工程师的时间应花在因子类型与滑窗策略上，而不是在类名海里迷路。

2. 因子图在最小化什么

变量是位姿、路标、bias、外参；因子是带噪声模型的残差。整体目标：

x min k \sum ∣ r_{k} (x; z_{k}) ∣_{Σ_{k}}^{2} = x min graph.error(values)

后端不「认识」相机或 IMU——它只吃因子。视觉投影、预积分、轮速、GPS 都是不同的 Factor 类；换传感器改因子类型，不改优化骨架。上次对接自研 UWB 锚点，也是写 RangeFactor 挂到同一 NonlinearFactorGraph 上。理解这一点后，读新传感器融合代码只需找「新增了哪些 Factor」。

3. 四个核心对象

Values 存当前估计；NonlinearFactorGraph 存约束；NoiseModel 定权重；优化器做 LM 或 iSAM2。

cpp

Values initial;
initial.insert(X(1), Pose3(...));
graph.add(PriorFactor<Pose3>(X(1), prior, prior_noise));
graph.add(BetweenFactor<Pose3>(X(1), X(2), delta, odom_noise));
graph.add(GenericProjectionFactorCal3_S2(...));
LevenbergMarquardtOptimizer opt(graph, initial);
Values result = opt.optimize();

noiseModel::Robust::Create(Huber, base) 包一层 M-估计；Sigmas 维数必须与残差维一致，否则运行时 assert。信息矩阵过大会把正常观测压死，过小则外点主导——Sigmas 来源（标定 Allan？经验 pixel？）要在代码里可追溯。

4. iSAM2 与 batch 的分工

全图 LM 在千帧规模不可实时。iSAM2 维护 Bayes tree，新因子只更新受影响子树——适合长期运行、多回环、多会话。插入频率高时 relinearize 开销主导；插入稀疏时 batch 与增量结果应一致。

cpp

ISAM2 isam(params);
isam.update(newFactors, newValues);
isam.update();  // relinearize
Values est = isam.calculateEstimate();

relinearizeThreshold 控制何时重线性化：太小 CPU 爆，太大线性化点陈旧、与 batch 差大。LIO-SAM 等长期运行用 iSAM2；短窗 VIO 前端仍常 Ceres batch——选型看因子插入频率，不是看「哪个库新」。对接时先用短 bag 对比 batch 与 iSAM2 轨迹，再开长期压测。

5. 读开源后端的固定顺序

我的顺序：① 找 Symbol 定义，弄清状态维数；② 列因子类型（Between / Imu / Projection）；③ 追噪声来源；④ 看滑窗 fix 与 marginalize 哪些 key；⑤ 确认 LM 还是 iSAM2。g2o 用 Vertex/Edge，语义相同；Ceres 更底层，因子结构不显式，但 cost function 列表等价于因子枚举。

6. Pose 约定与 gauge

GTSAM Pose3 是 body-in-world 还是 world-in-body，必须与前端一致——混了 BetweenFactor 残差会「能跑但漂」。无先验时整图有 6 自由度 gauge；至少一个 PriorFactor 或 fix 第一帧，否则 iSAM2 也会整体漂移。多会话合并时 gauge 更隐蔽：两图各自 fix 第一帧，合并边信息不足仍会扭。Marginalize 后旧 key 勿再 insert；长期运行里「幽灵变量」会导致 error 偶发跳变。

7. 常见症状

症状	常见根因
assert 噪声维数	Sigmas 长度 ≠ 残差维
优化发散	左右乘约定与前端不一致
一个外点毁图	未包 Robust noise
增量与 batch 差大	relinearize 阈值过大

8. 与 g2o / Ceres 选型

	GTSAM	g2o	Ceres
因子图原生	是	是	否
流形 Pose3	内置	SE3Quat	手动
增量	iSAM2	无	无

VIO 前端常用 Ceres（VINS-Mono）；LIO 后端常用 GTSAM（LIO-SAM）；ORB-SLAM 用 g2o。选型看增量需求与团队已有 wrapper，不是「哪个库新」。Ceres 灵活但因子结构不显式，Debug 时要自己画因子图。

9. 验收

graph.error() 优化前后应显著下降；
iSAM2 单次 update 耗时随新因子数线性可控；
同一 bag 上 batch LM 与 iSAM2 位姿差在毫米级（短序列对照）；
故意注入错误回环边，Robust 或 switch 应能抑制 ATE 突变；
marginalize 后旧 key 不应再被误 insert 进 Values。

因子图语言一旦对齐，读任何 GTSAM/g2o 后端都是同一套问题结构——变量、残差、噪声、滑窗，而不是在类名里迷路。新人 onboarding 时，用一条短 bag 画出 factor 列表比讲一天类继承树更有效。Batch 与 iSAM2 结果对照应成为合并后端的常规 smoke test。