Incremental smoothing

SLAM 是一个不断增长的估计问题:每一个新的关键帧都会向因子图中添加变量和因子。每一步都从头求解整个非线性最小二乘问题(批量优化)的代价会随轨迹增长而越来越高——但每一个新测量通常只影响图中一小部分、最近的区域。**增量式平滑(incremental smoothing)**正是利用了这一点:只用新信息去更新之前的解,而不是重新计算全部内容。

线性代数视角

求解线性化后的最小二乘系统,意味着对测量雅可比矩阵进行分解($J = QR$,再回代求解 $R\,\Delta\mathbf{x} = \mathbf{d}$),或者对信息矩阵进行分解($H = J^TJ$,通过 Cholesky 分解)。一个新的测量会向 $J$ 追加新的行。iSAM(增量式平滑与建图,incremental Smoothing and Mapping,Kaess 等人)的关键洞见是:现有的三角因子 $R$ 可以针对这些新增的行通过一系列 Givens 旋转进行更新——一种低秩修改——而不需要从头重建。由于这个更新只会填充与新测量的变量相连的条目,$R$ 的大部分内容都不受影响。iSAM 仍然需要周期性的批量步骤来重新线性化和重新排序变量,因为线性化点和变量排序(它控制着填充量)都会随着估计值的演变而逐渐过时。

iSAM2 与贝叶斯树

iSAM2 通过引入**贝叶斯树(Bayes tree)**去掉了对周期性批处理的依赖——这是一种从因子图上的变量消元派生出的树结构数据结构:将消元运行到完成会得到一个贝叶斯网络,其团(clique)组织成一棵有向树。贝叶斯树精确地编码了哪些变量的解依赖于哪些其他变量——每个团的解只依赖于它朝向根方向的祖先。

当一个新因子到达时:

• 只有从受影响变量到根的路径上的团会被重新消元——树的其余部分(通常是绝大多数)不受影响。
• 流动式重线性化(fluid relinearization):一个变量只有在其估计值相较于上一次线性化点移动超过某个阈值时才会被重新线性化,因此非线性问题只在真正需要的地方(也仅在那里)被处理。
• 部分状态更新:解的更新从被修改的团向下传播,并在变化低于某个阈值时停止,因此只有真正发生变动的估计才会被重新计算。

其结果是:对于探索型轨迹,更新几乎是常数时间的,同时在线性化阈值范围内保持精确——这不是一种会丢弃信息的固定滞后近似;整个变量集合上的完整平滑问题被始终维持着。较大的回环闭合仍会触及树的很大一部分(这是正确的,因为它们确实会影响一切),这在性能分析中会表现为偶尔出现的延迟尖峰。

选择后端风格

iSAM2 在 GTSAM 中实现,是当你需要在实时速率下进行全历史平滑时的首选后端:例如,Kimera-VIO 使用 GTSAM 的 iSAM2,配合无结构的”智能”投影因子进行增量式视觉惯性估计,而 LIO-SAM 也将其激光雷达-惯性因子图送入同一个求解器。另一种主要的后端风格——带边缘化的固定滞后滑动窗口——通过丢弃旧变量来限制代价;增量式平滑则通过巧妙的更新在保留旧变量的同时限制代价。

实践要点

• 重线性化阈值是在速度和精确度之间做权衡;过于宽松的阈值会在高曲率(快速旋转)的路段悄悄降低精度。
• 无结构/“智能”因子(在因子内部解析地消去路标)能让贝叶斯树在具有大量短生命周期路标的视觉问题中保持较小规模。
• 如果你的平台无法容忍回环闭合带来的延迟尖峰,常见的架构是:一个快速的固定滞后里程计前端,加上一个在其后运行的 iSAM2(或位姿图)平滑器。

对SLAM的意义

增量式平滑正是”平滑胜过滤波”这一洞见在实时速率下变得可行的原因:你能在整条轨迹上获得完整非线性优化的精度,而不需要在每一帧都支付批处理的代价。当你看到一个系统被描述为”基于 GTSAM/iSAM2”时,你现在就知道它的计算故事是什么:贝叶斯树更新、流动式重线性化,以及精确的全历史 MAP 推断。它是需要将回环闭合无缝融入的 VIO 和激光雷达-惯性系统(例如 LIO-SAM、Kimera)的标准后端。

相关条目

• Factor graph
• MAP inference as sparse nonlinear least squares
• Marginalization
• Kimera-VIO
• LIO-SAM