GTSAM 官方流形优化工作流:构建因子图、在线性空间求解并映射回流形

GTSAM

Stars

Georgia Tech Borg Lab 面向机器人与计算机视觉的平滑和建图库,以因子图与贝叶斯网络表达状态估计问题,提供 C++ 核心以及 Python、MATLAB 接口。

Factor GraphsSmoothing · MappingC++ · Python · MATLAB
FACTOR GRAPH · FIXED-LAG LIFECYCLE

值可以先到,因子可以晚到

等待连接的 value 既不能提前删除,也不能在过期后被送进一个要求 Bayes-tree clique 的边缘化路径。

TIME-ORDERED UPDATES · LAG = 1.0
t0t1t2t3
X0
VALUE + PRIORconnected · clique ✓
X1
VALUE FIRSTfactor ∅ · clique ∅
X2
CLOCK ADVANCESX1 leaves lag window
ACTIVE KEY SPLIT
new factor keys+VariableIndex
is key active?
BEFOREall expired → marginalizeLeavesclique required ✕
AFTERconnected ≠ pendingtwo safe lifecycles ✓
FIXED-LAG LIFECYCLE
FACTOR ARRIVES IN WINDOW
X0— factor —X1
CONNECT · OPTIMIZE · MARGINALIZE
STILL UNCONNECTED AT EXPIRY
X1remove valueerase timestamp
REAP DIRECTLY · NO BAYES CLIQUE
异步输入

Value 已存在,Factor 仍在路上

`ISAM2` 允许先把 value 放入 `newTheta`、再由后续更新中的 factor 建立连接;这类 pending value 存在线性化点中,却暂时没有 `VariableIndex` 条目或 Bayes-tree clique。旧固定时窗逻辑会把所有过期时间戳键都送入排序与叶节点边缘化,使从未连接的 value 在需要 clique 的路径中失败。

状态区分

Pending 与 Connected 必须分流

“尚未连接”与“已经失效”是两个不同状态:value 在 lag window 内必须继续保留,以便迟到的 factor 仍能接入;只有超过时窗且依旧没有 factor 或 clique 时才应直接回收。同时,当前更新中新到达的 factor 必须立刻把对应键视为 active,不能被同一轮过期判断误删。

时窗语义

时窗内保留,过期后完整回收

从新 factor 与现有 `VariableIndex` 建立 active key 集合,将过期键拆成 connected 与 pending 两组:前者继续走约束排序和 `marginalizeLeaves`,后者直接从 ISAM2 value、增量状态及时间戳映射中清理;并让变量删除逻辑兼容从未拥有索引条目的 pending value。

图结构安全

无 Clique 的变量不再进入边缘化

固定时窗平滑器现在支持 value 与 factor 异步到达:时窗内迟到的 factor 可以正常连接,始终未连接的 value 则会在过期后完整回收且不触碰 Bayes-tree 边缘化。两条生命周期均有回归覆盖,相关四组测试与 Linux、macOS、Windows CI 全部通过,修复 #2741。