Home / 机器人经典基础 / iSAM2：把不断增长的SLAM问题变成局部树更新

068｜iSAM2：把不断增长的SLAM问题变成局部树更新

原论文： iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，Michael Kaess、Hordur Johannsson、Richard Roberts、Viorela Ila、John Leonard、Frank Dellaert。本文解读2012年IJRR期刊版；期刊在线发表于2011年12月，另有2011年ICRA先行论文。期刊链接 · 作者全文。

先接住前作的问题

iSAM能增量更新线性分解，非线性重排和重线性化仍可能触及大范围。iSAM2把分解转为Bayes tree编辑。

五个前置概念

理解本文需要的五个概念：

因子图：用局部函数连接变量，表达测量如何约束状态。
高斯—牛顿：线性化残差并迭代求解局部最小二乘。
消元与填充：消去变量会在剩余变量之间产生新联系。
Bayes树：用条件概率团组织消元结果，暴露可复用结构。
回代：已有三角分解后，按依赖关系恢复变量增量。

一、问题设定：新增约束怎样只重建受影响的稀疏估计结构

机器人每走一步，就多一个位姿和若干测量。最准确的做法之一是保留历史位姿与地图，把所有约束联合优化；但每一步从头求解全部历史，计算会越来越重。原版iSAM可以增量更新矩阵，却仍需要周期性的重排序和重新线性化。

研究背景与问题定义

这是一篇状态估计与稀疏优化论文，不是强化学习论文。没有策略、奖励或训练回报；输入是传感器处理后得到的测量因子，输出是机器人轨迹和可选地标的估计。数据关联通常已经由前端处理，iSAM2并不自动判断“这次看见的是不是过去那个地标”。

变量集合 $\Theta$ 可包含位姿和地标。测量 $z_i$ 约束其中一个小子集 $\Theta_i$ 。高斯噪声假设下，最大后验估计可以写成加权非线性最小二乘：

$\Theta^\star=\arg\min_\Theta\sum_i \|h_i(\Theta_i)-z_i\|_{\Sigma_i^{-1}}^2.$

$h_i$ 是测量预测， $\Sigma_i$ 是噪声协方差，平方范数给不同方向与测量不同权重。通常还需一个先验固定全局坐标自由度，否则整体平移或旋转不会改变相对测量，系统可能奇异。

全量平滑保留过去状态，能够用后来的回环修正早期轨迹。困难在于：新增约束影响哪些变量？何时旧线性近似已不可靠？变量按什么顺序消去才不会产生大量填充？这些就是本文的研究对象。

二、以Bayes tree管理消元与流动重线性化

iSAM2的关键是把稀疏矩阵分解解释成一棵 Bayes tree。新测量到来时，找出受影响的树部分，重建它，并把未变的子树接回。再根据估计变化决定哪些变量需要重新线性化，按需回代更新状态。

1. 从因子图到Bayes树

在线性化点 $\bar\Theta$ 附近，用局部增量 $\delta$ 近似残差：

$\delta^\star=\arg\min_\delta\|A\delta-b\|^2,\qquad \Theta\leftarrow\bar\Theta\oplus\delta.$

$A$ 是白化后的稀疏雅可比， $b$ 是负残差； $\oplus$ 表示把切空间增量映射回位姿等变量，旋转不能简单当普通向量无限相加。

变量消元得到条件概率的乘积。Bayes树把它们组织为团：每个团包含自己的 frontal variables，以及作为条件的 separator variables，可写为 $p(F_k\mid S_k)$ 。全局密度由这些条件相乘构成。它与平方根信息矩阵的块三角结构相对应，不是用来存储地图几何的八叉树。

2. 更新树的上部，保留未变子树

新因子涉及某些变量，它们所在团及到根的祖先需要重新消元。把这部分恢复成因子图，加入新测量，重新排序、消元、构造新树，再接回未受影响的“孤立”子树。边界上的缓存因子也必须保留，否则过去子树的信息会丢失。

局部更新的依据是条件独立性，不是“离机器人远就忽略”。一个跨越地图的大回环，可能把很大的树部分联系起来；反之，有些几何上很近的变量在消元结构中并不便宜。

3. 增量排序与流动重线性化

消元顺序决定 fill-in：原先无直接连接的变量，可能因消元产生额外连接。iSAM2在受影响部分使用受约束的近似最小度排序，让近期访问变量倾向靠近消元末端、树根附近，为接下来的更新降低成本。最小填充排序本身一般很难，启发式不提供普遍最优保证。

非线性问题还要决定何时重新计算雅可比。若变量当前增量超过阈值 $\beta$ ，更新其线性化点并标记相关团。论文称为 fluid relinearization。它不是完全放弃重线性化，而是避免每次对所有变量统一执行。

另一个阈值 $\alpha$ 用于部分回代：树中传播的解变化足够小，就不继续深入该分支。 $\alpha$ 控制解更新传播， $\beta$ 控制线性模型刷新，二者不能混为一个“误差阈值”。不同量纲也需要合理尺度，米和弧度不能无解释地共用某个物理精度承诺。

输入：当前Bayes树、原始非线性因子、线性化点、上次增量
收到新因子与新变量：
    保存因子，并初始化新增变量
    根据增量阈值标记需要重线性化的变量
    合并新因子涉及的变量，找出相关团及其到根的路径
    移除这些团，保存未受影响子树与边界缓存因子
    在更新的线性化点重建所需线性因子
    对局部变量做受约束排序、消元，形成新的树上部
    接回原有子树
    从根向下回代，按变化阈值停止不必要传播
    返回线性化点加局部增量得到的轨迹和地图

以上重构原文算法4—8的逻辑。数学优化目标没有换，改变的是计算组织、线性化更新与求解近似。改进指标是时间、重新计算条目数和目标值接近程度，不是“使用更少环境交互学会任务”。

“局部改树”不等于“只会改变局部位置”

假设三个标量状态的条件分解为 $p(x_1\mid x_2)p(x_2\mid x_3)p(x_3)$ 。新因子只连接 $x_3$ 时，前两个条件式可以保留，但 $x_3$ 的最优值改变后，回代得到的 $x_2,x_1$ 仍可一起移动。教学例条件均值关系 $x_2=x_3-1,x_1=x_2-1$ ，根从2变为3，则整条解由(0,1,2)变为(1,2,3)，并不要求重新分解每个条件因子。

原文第5–6页将新增因子所触及团到根的路径移除重建，未受影响子树随后接回。这节省的是分解工作，不是强制远处地图保持原值。流形重线性化又多一层近似，阈值越宽松，计算越少但旧局部模型可能偏得更多。作者期刊稿Bayes tree更新部分

本篇直接承接iSAM；后作IMU预积分和LIO-SAM可以将其作为增量后端。大回环牵动高层团时，更新时间会突增，因此实时性应看尾部延迟，而不仅是均值。

受影响子树的编辑与阈值重线性化节约不同计算。下面检查两者怎样协同回代，再比较目标值、平均耗时和回环峰值。

三、编辑受影响子树，再按阈值回代

自构例子，未运行。 一辆车先走左侧房间，再走右侧房间。新里程计只连接最后两个位姿，可能只改树根附近。随后发现右侧房间其实与最早位置相连，就要沿相关祖先路径重新组织信息。左侧某些已消元的子树若条件关系没变，可接回使用；这不意味着左侧最终位姿绝不会改变，回代仍可能把新的边界条件传下去。

阅读范围：核对作者提供的期刊稿中§3—5、算法4—8、图 5–图 16与表 1；未独立复现，未重新计时，教学例子未运行。

原论文图5：逐步加入约束时Bayes树中受影响团与未受影响子树 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 5，PDF 第 7 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：红色部分标出需重新消元的范围，灰色子树可被复用。

原论文图6：由Bayes树取出受影响部分、更新因子后重新接回子树 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 6，PDF 第 8 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：按四个阶段追踪同一组变量，子树保留的是其条件关系。

原论文图7：不同变量消元顺序对应的因子图与Bayes树 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 7，PDF 第 8 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：蓝色椭圆所示变量组合改变团结构，排序会影响后续更新工作量。

原论文图8：增量排序的地图示例、非零矩阵条目与受影响变量数 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 8，PDF 第 10 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：填充量与受影响变量计数是计算工作指标，不是独立定位真值误差。

原论文图9：重线性化阈值变化下的归一化目标差异与计算条目 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 9，PDF 第 11 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：较大的阈值可减少更新工作，但上图也需同时检查求解质量。

原论文图10：回代阈值变化下的目标差异与受影响变量数 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 10，PDF 第 11 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：两幅图把节省的工作和代价变化并列展示，不能只看速度一侧。

原论文图11：City20000、W10000、Intel和Killian Court四组测试地图 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 11，PDF 第 12 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：这些是不同结构和规模的数据集，不能把跨数据集耗时当成同难度比较。

原论文图12：Trees10000合成地标场景与Victoria Park实测轨迹 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 12，PDF 第 15 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：上图为合成结构、下图为真实场景，两种证据的来源不同。

原论文图13：Sphere2500与Torus10000优化前后的三维结构 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 13，PDF 第 13 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：左右的整齐程度对应优化前后几何，不能据此替代独立真值误差统计。

原论文图15：iSAM2计算时间在消元、重线性化、求解与重排序中的分配 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 15，PDF 第 15 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：扇区百分比说明该测量设置下的耗时构成，不是所有图问题的固定比例。

原论文图16：不同优化器相对充分优化参考解的归一化卡方差异 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 16，PDF 第 16 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：参考解是同一目标充分优化后的结果，不是外部动作捕捉真值。

iSAM2所有数据集的逐步耗时和累计耗时八组面板 — iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree，图 14，PDF 第 14 页；原文与图注。原版权保留。本图为裁切摘录，按CC-BY-NC-SA-3.0提供；版本与许可记录。

读图：前两组同时给出每步和累计时间，其余六组比较累计时间；同一幅图覆盖不同规模和结构的数据集，不能只凭第一组代替全部比较。

四、比较精度、更新成本与大回环峰值

Fig. 14(a)：左侧每步耗时有明显尖峰，右侧累计时间反映总工作量 — 原论文结果图／表摘引：Kaess 等（2012），Fig. 14(a)，PDF 第 14 页；核对原文。原版权保留。

读图：左侧每步耗时有明显尖峰，右侧累计时间反映总工作量。City20000 的这个对照提醒我们：平均可在线与最坏延迟可满足控制期限是不同要求，也不能预设 iSAM2 始终最快。

实验如何验证方法

项目	原文设置	验证问题
二维位姿图	City20000、W10000、Manhattan、Intel、Killian Court	图规模和约束密度怎样影响增量成本
带地标问题	Trees10000、Victoria Park	不只适用于纯位姿图
三维问题	Sphere2500、Torus10000	大范围三维旋转下的非线性求解
对比	iSAM1、HOG-Man、SPA；部分不支持某类问题	同输入约束上的效率与目标值
硬件	Intel 1.6 GHz i7-720笔记本；iSAM2单线程C++研究实现	这是特定实现的墙钟测量
iSAM2参数	$\alpha=0.001,\beta=0.1$ ；每10步检查重线性化	速度与近似精度折中
排序与求解	CCOLAMD；论文实现使用团内Cholesky方案	不把抽象QR推导当唯一实现
指标	每步时间均值、标准差、最大值；总时间；归一化 $\chi^2$	同时检查平均速度、延迟峰值与约束拟合

论文§5明确提醒实现差异会影响速度，所以还测量受影响变量及稀疏矩阵重新计算条目。表 1的时间标准差描述各增量步骤的运行波动，不应自动解释为多随机种子重复实验的方差。本文未逐项确认额外重复运行次数。

结果：证据到底有多强

证据1：图 5–图 6解释局部性的来源。 新回环只要求重建树的一部分，未影响子树能够复用。这是算法结构示例，说明为什么可以节约计算，不是任何SLAM图都只改固定大小区域的证据。

证据2：图 8–图 10分开检查排序与阈值。 图 8比较增量排序导致的填充及受影响变量；图 9研究重线性化，图 10研究回代阈值。Manhattan上小阈值能够维持接近充分求解的目标值并减少工作量。不能据此把同一阈值无条件迁移到不同单位、噪声和图结构。

证据3：表 1并没有“全面更快”。 Intel上iSAM2总时间1.59秒，iSAM1为5.28秒，约节省69.9%；Torus10000为352对864秒。但City20000是323对141秒，iSAM2反而更慢；W10000上HOG-Man164秒比iSAM2的224秒快。重线性化提高精度，也可能多付计算，必须与精度一起比较。

证据4：表 1与图 14展示延迟峰值。 City20000中iSAM2平均每步16.1毫秒，最大1125毫秒。平均看起来可在线运行，不意味着严格实时控制期限都满足。大回环、较大重建区域与实现常数都会影响峰值，移植到机器人时需要缓冲或异步架构。

证据5：图 16的“真值”是优化参考解。 图中参考线来自每步充分迭代到收敛的最小二乘估计，比较的是归一化 $\chi^2$ 差异，不是外部动作捕捉真值。iSAM2更接近它，支持求解质量；错误的数据关联或噪声模型即使被精确优化，也可能得到错误地图。小尖峰与每10步重线性化的安排有关。

五、检查近似阈值和非线性收敛条件

局限也来自同一结构。图越来越大，历史状态与因子仍需存储；作者明确说无限运行的“永久SLAM”仍是开放问题。大回环可能影响广泛变量，一般复杂度上界并非常数。初始化足够好、局部线性近似有效及高斯测量模型，都是解释结果的重要条件。

本文的进一步分析：后端拟合优良不能替代前端可靠性；离群回环可能让求解器迅速而精确地优化错误问题。接入系统时，还需单独处理鲁棒损失、数据关联、异常测量和生命周期管理。这些不是本文已经解决的全部功能。

六、追问：把重线性化阈值调大后速度提高，应怎样确认付出的精度代价？

把重线性化阈值调大后速度提高，应怎样确认付出的精度代价？

参考答案：固定因子、初始化和数据顺序，比较最终目标、状态误差及需要更新的变量范围，而不能只报告平均时间。阈值决定哪些变化暂时不重算，大回环可能放大这项取舍。接近充分迭代结果是实验结论，不能当所有阈值的保证。

七、让高频测量与增量后端配合

模块组合：IMU 预积分：把高频惯性测量压缩成可以反复优化的约束。用预积分减少惯性因子内部的重复计算。
模块组合：LIO-SAM：让激光、惯性、GPS 与回环共同解释运动。在激光惯性系统中作为增量后端使用。

八、自测与参考答案

问题1：为什么保留未修改子树，不等于忽略它们的历史信息？

答案：子树已经编码为条件关系，并通过缓存的边界因子影响重新消元的区域。新树接回它们后，新的边界估计仍可通过回代影响内部变量。复用的是计算结果和条件结构，不是删除过去观测。

问题2：iSAM2目标值接近参考最小二乘，为什么不能证明地图接近真实世界？

答案：参考解只保证更充分地满足输入因子。若回环关联错误或协方差不合理，目标本身就偏离真实问题。应另用独立真值轨迹或几何测量评价地图，而不能把优化残差当物理真值误差。

问题3：平均每步16.1毫秒，能否据此接进要求每20毫秒必出结果的控制循环？

答案：不能。论文同数据集最大步骤超过一秒，平均速度不约束尾部延迟。必须实测最坏或高分位延迟，并考虑异步后端、预测状态或计算预算；这些系统设计不由平均值自动保证。