说句实话从这一篇开始GTSAM学习系列才算真正摸到了“在线 SLAM”的核心。前面我们一直拿因子图和批量优化在玩数据量小的时候完全够用但一旦你手上是一个持续运行的机器人位姿不断新增、回环不断闭合还指望每次来一个新观测就把整张图重新解一遍那实时性基本就泡汤了。ISAM2全称 Incremental Smoothing and Mapping第二版就是 GTSAM 里专门解决这个问题的增量求解器。它并不追求每次都对全部变量做一次完整优化而是只更新那些“被新因子影响到的局部变量”从而把每次更新的开销降到一个很低的水平。这篇文章我从理论到代码把 ISAM2 掰开揉碎讲清楚适合已经能跑通基础因子图的同学也适合那些听过 iSAM 和 ISAM2 名字、但一直搞不清两者区别的工程师。1. 为什么需要ISAM2从批量优化到增量优化1.1 先回顾一下因子图优化到底在做什么因子图这个东西我们前面文章已经反复用过。一个典型的 SLAM 因子图变量节点就是机器人的位姿和路标位置因子节点则是各种约束比如里程计给出的相邻位姿之间的相对变换、激光匹配给出的当前位姿与某个路标之间的观测关系、闭环检测给出的两个历史位姿之间的相对约束等等。整个优化问题的目标就是找到一组变量取值使得所有因子的残差平方和最小。本质上是一个非线性最小二乘问题。在 GTSAM 里批量求解的典型做法是高斯牛顿或者 LM 算法先给所有变量一个初始值然后在当前线性化点附近把误差函数展开成一阶泰勒级数得到一个形如 Aδ b 的线性系统求解这个系统得到变量增量 δ更新变量后继续迭代直到收敛。这个过程在每一步都要把整个问题的雅可比矩阵 A 组装出来然后做一次分解比如 QR 分解或者 Cholesky 分解。对于几百个变量的离线建图任务这个开销完全能接受CPU 上也就是几十毫秒的事情。但实时系统不一样。假设一个机器人以 10Hz 的频率运行每秒钟新增 10 个位姿节点和若干观测因子那张图会不断膨胀。如果每次都把整个 A 矩阵重新组装、重新做分解计算量会随变量数量快速增长。做过实验的人可能都有印象批量求解在变量数从几百涨到几千的时候耗时可能从几十毫秒涨到几秒钟这个增长速度是没法接受的。1.2 批量求解的瓶颈到底卡在哪里批量优化的核心瓶颈在于它没有利用“时间连续性”。上一时刻我刚解完一次得到了当前所有变量的估计值下一秒来了一个新的里程计因子和一个新的位姿节点按理说大部分旧变量的最优值并不会发生太大变化真正受影响的往往只有新节点附近的一小撮变量。但批量优化不管这些它每一次都从零开始把整张图的雅可比重新组装一遍再做一次全局分解。这个“每次全量重算”的成本在稀疏问题里主要体现在矩阵分解这一步。虽然因子图对应的雅可比矩阵是稀疏的利用稀疏 Cholesky 分解可以一定程度上降低复杂度但变量规模大了以后分解过程仍然非常昂贵。而且从工程实现角度来说每次都要从数据结构层面把整个图重新遍历一遍也会带来不少开销。所以业界一直在想能不能把上一次求解得到的分解结果缓存下来新因子来了以后只在局部做修改而不是重新做一遍全局分解这个思路就是增量平滑与建图的起点。iSAM1 已经尝试了这个方向但它在线性化处理上不够灵活整体还需要定期全量重做ISAM2 则在数据结构和更新策略上做了本质性的改进。1.3 ISAM2 的“增量”到底体现在哪里ISAM2 的核心思想可以概括成一句话尽量复用之前已经算好的分解结果只对受新因子影响的那部分变量重新做线性化和消元然后局部更新整个解。在具体实现上GTSAM 的 ISAM2 每次调用 update 时并不是把所有因子都拿过来重新处理而是把新增的因子先放进一个临时因子图里判断它们涉及哪些变量进而决定需要更新贝叶斯树上的哪些子树只重算这些受影响的部分。要做到这一点光靠传统的因子图表达是不够的ISAM2 引入了一个关键数据结构贝叶斯树。这也是这一篇理论部分的重头戏。说白了因子图负责“描述问题”贝叶斯树负责“记录上一次求解的中间结果”并且这个中间结果被组织成了方便局部修改的形式。下次再来新因子ISAM2 只需要找出贝叶斯树上对应的局部范围把这一小块重新消元再接回原来的树上就完成了增量更新。这就好比一本书的目录你只改其中一章不需要把整本书重新排版。2. 贝叶斯树ISAM2的理论脊梁2.1 变量消元因子图如何变成贝叶斯网在理解贝叶斯树之前得先理解变量消元的过程。我们面对的因子图是由变量和因子组成的二分图。求解时通常用高斯牛顿法线性化后得到一个稀疏线性系统然后对这个系统的系数矩阵做 Cholesky 分解得到上三角矩阵 R。这个过程在概率图模型的角度看其实就等价于对因子图做变量消元每消去一个变量就把它涉及的所有因子乘在一起然后对其中一个变量求条件分布得到一个条件概率项剩下的变量继续消直到所有变量都被消完。消完后得到的是一串条件概率的集合每个条件概率对应一个“父节点变量给定时子节点变量的分布”这正好是一个贝叶斯网的结构。这个贝叶斯网完整地编码了原问题在当前线性化点下的所有信息。以后再要求解只需要在这个贝叶斯网上做一次回代就能得到所有变量的估计值不需要重新对因子图做全局消元。这个思想特别重要因子图和贝叶斯网是同一份信息的两种表达方式因子图面向“描述问题”贝叶斯网面向“快速求解”。把因子图消成贝叶斯网的过程相当于提前把矩阵分解做完了后续的求解就是纯粹的回代成本很低。2.2 贝叶斯树的构造逻辑贝叶斯树可以理解为贝叶斯网的一种树状重组。它并不是一个新的概率模型而是对贝叶斯网中的条件依赖关系按“消元顺序”重新组织。树上的每个节点实际对应的是一个小的“条件概率团块”也就是变量消元过程中被消去的一部分变量的联合条件分布。节点之间的父子关系体现了变量的条件依赖方向。之所以要换成树结构是因为树有很好的局部性。你想想看一棵树里从根节点到任意叶子节点只有唯一路径这意味着一棵子树只跟它的父节点和祖先节点有直接关联。新因子到达后我们可以精准判断它影响了哪个区域的变量从而找出贝叶斯树上对应的那棵子树只重新消元这一棵子树然后替换掉原来的子树。从算法实现角度来说新因子加入后ISAM2 会在现有的贝叶斯树上做一次“剪枝”操作把包含受影响变量的那些团块从树上拆下来把这些团块里的因子加上新因子一起重新消元得到一组新的团块再按原有的消元顺序重新接回树里。这个操作在 GTSAM 里对应的是isam.update()内部的核心流程对用户来说是透明的但理解它有助于你调参。2.3 增量更新与流体重线性化前面说的增量更新默认了一个前提新因子到来时旧的线性化点仍然足够准确。但非线性系统里这个假设经常不成立。随着新观测不断加入变量真实值可能已经偏离了上次线性化时用的线性化点这时候如果还拿旧的雅可比去近似误差函数增量更新的效果就会变差甚至可能不收敛。经典的解决方法就是“重新线性化”。最简单粗暴的做法是每来一批新因子就把所有变量重新线性化一遍但这样成本太高失去了增量更新的意义。ISAM2 的做法是“流体重线性化”不全局重做而是只检查那些可能已经显著偏移的变量如果某个变量当前估计值与上次线性化点之间的距离超过阈值就把该变量相关的因子重新线性化。阈值就是ISAM2Params里的relinearizeThreshold默认一般是 0.1单位是变量在当前线性化点下的增量大小。这个设计非常巧妙。在实际 SLAM 场景里大部分变量在大部分时间内变化都很小只有回环闭合或者剧烈运动时才会出现局部突变。正是“哪些需要重线性化就只重做哪一块”的流体策略让 ISAM2 在保持精度的同时计算量始终能被压住。后来使用 ISAM2 时这个relinearizeThreshold是我最常调的关键参数之一它对精度和耗时的影响非常明显后面章节专门说。3. GTSAM实操ISAM2最小骨架与参数配置3.1 最小可运行代码骨架理论部分说得再多最后还是要落到代码上。GTSAM 里使用 ISAM2 其实非常简洁核心只涉及三个接口构建ISAM2对象、调用update传入新因子和新变量、调用calculateEstimate获取当前最优估计。下面是一个最基础的一维位姿链示例的骨架。#include gtsam/geometry/Pose2.h #include gtsam/inference/Symbol.h #include gtsam/nonlinear/ISAM2.h #include gtsam/nonlinear/NonlinearFactorGraph.h #include gtsam/slam/BetweenFactor.h #include gtsam/slam/PriorFactor.h #include gtsam/nonlinear/Values.h using namespace gtsam; using gtsam::symbol_shorthand::X; int main() { // 1. 配置 ISAM2 参数 ISAM2Params params; params.relinearizeThreshold 0.01; params.relinearizeSkip 1; ISAM2 isam(params); // 2. 初始先验 NonlinearFactorGraph newFactors; Values initial; auto priorNoise noiseModel::Diagonal::Sigmas( (Vector(3) 0.1, 0.1, 0.1).finished()); newFactors.addPrior(X(0), Pose2(0, 0, 0), priorNoise); initial.insert(X(0), Pose2(0, 0, 0)); isam.update(newFactors, initial); newFactors.resize(0); initial.clear(); // 3. 增量加入里程计因子 auto odomNoise noiseModel::Diagonal::Sigmas( (Vector(3) 0.05, 0.05, 0.05).finished()); for (int i 1; i 100; i) { Pose2 odom(1.0, 0.0, 0.0); newFactors.addPrior(X(i), isam.calculateEstimate().atPose2(X(i)), nullptr); // 占位演示实际用 BetweenFactor newFactors.add( BetweenFactorPose2(X(i - 1), X(i), odom, odomNoise)); // 用运动模型预测新节点初始值这里用滑窗里的里程计递推 Pose2 prevPose isam.calculateEstimate().atPose2(X(i - 1)); initial.insert(X(i), prevPose * odom); isam.update(newFactors, initial); newFactors.resize(0); initial.clear(); } Values result isam.calculateEstimate(); result.atPose2(X(99)).print(); return 0; }上面这段代码里有个地方是我故意留的“坑”newFactors.addPrior(X(i), ...)那一行在真实工程里是不应该出现的。我把它留在示例里是想强调一个非常容易犯的错误新因子必须和新变量、新约束严格对应不要顺手加一些多余的因子进去。实际项目中正确写法应该是只添加BetweenFactor配合initial.insert(X(i), 预测位姿)然后交给isam.update。3.2 ISAM2Params 关键参数逐项过一遍ISAM2 真正需要用户干预的地方就是初始化时的参数配置。GTSAM 提供了ISAM2Params这个结构体来管理这些选项大多数字段有合理的默认值但有几个参数在实际工程中非常值得单独调一调。首先是relinearizeThreshold它控制流体重线性化的阈值。默认值是 0.1意味着如果某个变量的估计值相对上次线性化点的增量超过 0.1这个变量相关的因子就会被重新线性化。这个值越小重新线性化越频繁精度理论上越高但计算量也越大如果设得太小比如 0.001ISAM2 几乎每一步都在重做局部线性化增量的优势就削弱了。我一般建议在 0.01 到 0.1 这个区间试先跑一遍离线数据对比误差和耗时再决定。其次是relinearizeSkip表示每隔多少次update才执行一次重新线性化检查。默认是 10即每十次更新里只有第一次会检查哪些变量需要重线性化后面九次直接沿用旧的线性化点。这个参数的作用是节省检查开销。如果你的传感器频率很高、变量变化不大保持默认或者调大到 20 都没问题但如果在回环密集的场景我反而建议调到 1让每次更新都及时响应非线性变化。还有一个值得提的是enableRelinearization这个总开关默认是打开的。有些同学在调试时会为了“省时间”把它关掉结果发现误差怎么也降不下去。这个开关一般保持默认开启即可。另外cacheLinearizedFactors也很实用它会缓存已经线性化过的因子跳过重复的线性化计算默认开启显式关闭的场景非常少。最后如果你发现 ISAM2 输出结果和批量优化差异比较大可以打开evaluateNonlinearError让每次update返回误差变化量方便定位问题但会增加一点点计算开销。3.3 与iSAM1、批量求解的差异对比很多初学者会把 iSAM1 和 ISAM2 混为一谈但其实两者在数据结构上有本质区别。iSAM1 的思路是保留一个随时间更新的线性系统配合对变量消元顺序的动态调整但它依然需要在每次更新时维护一个全局的线性化状态回环一多代价就上来了。ISAM2 引入了贝叶斯树把线性化信息结构化地缓存起来更新操作变成针对局部子树的操作灵活性和效率都高不少。批量求解、iSAM1、ISAM2 三者的差异我觉得可以用下面这个表来总结求解方式每次更新成本精度适用场景批量高斯牛顿全局组装加分解复杂度高最高全图精确收敛离线建图、小规模问题iSAM1增量更新但线性化点管理笨重中等历史遗留项目已被ISAM2取代ISAM2只更新受影响子树高接近批量实时SLAM、长期运行值得强调的一点是ISAM2 的最终结果并不保证和批量优化完全一致尤其是在非线性很强、存在多峰分布的场景。因为两者的线性化点和迭代路径不同最终可能收敛到不同的局部极小值。只要你把 ISAM2 的重线性化阈值设得足够小、给足更新次数多数实际问题里两者结果非常接近。4. 实际调参与性能经验4.1 什么时候选ISAM2而不是批量优化这里我的建议很直接如果你的问题规模只有几十个变量、且对实时性没有硬性要求直接用批量优化就好代码更简单、理论更透明。ISAM2 的价值只有在“持续增长”这个问题形态下才真正体现。我举一个典型场景。室内移动机器人激光雷达 10Hz里程计 100Hz跑 10 分钟大概会产生几千个位姿节点和上万个因子。如果用批量优化每来一帧激光就要全量重解一次CPU 直接爆掉用 ISAM2每次更新只涉及新位姿附近的一小部分变量单次更新的耗时能稳定在毫秒级这才有资格上实时控制系统。换句话说ISAM2 适合的是“图会不断变大”的在线场景批量优化适合“图一次性给定、反复查询”的离线场景。4.2 重线性化阈值和跳数的实验结果我手头有一个室内数据集大概 1500 个位姿中间有 8 次回环闭合。我在相同数据和相同噪声模型下对比了几组参数的实际效果。第一组是relinearizeThreshold 0.1, relinearizeSkip 10整体耗时最低但回环闭合之后轨迹的累计误差要经过很多步才能慢慢消掉。第二组是relinearizeThreshold 0.01, relinearizeSkip 1单次更新耗时大概增加了 20% 到 30%但回环闭合后误差下降非常快最终轨迹和批量优化结果几乎重合。从这件事我得到一个经验当你的场景里有较强的非线性因素比如大幅度的转弯、频繁的回环不要为了省一点点耗时把阈值设得太大。反过来如果传感器噪声小、运动平缓阈值大一点也完全没问题。实际项目中我通常是先设0.01和1跑通整个流程后再根据帧率压力一点点放宽。4.3 长周期运行后的性能维护即便是 ISAM2长时间运行后也会遇到性能退化。原因是贝叶斯树会随着变量数量增加而变得很庞大即使每次只更新局部整棵树的查询、遍历成本也在上升。更关键的是频繁的回环闭合会把原本互不相邻的子树拼接在一起导致一次回环更新要重做很大一块区域单次更新耗时可能会暴涨一个数量级。对这种问题我目前用过比较稳的方案是“定期批量重置”。具体做法是每积累 N 个新节点或者每当累计误差超过某个阈值就把当前 ISAM2 的估计值取出来用这个估计值作为初始值重新构造一个非线性的因子图把已有的历史信息用估计值和边缘协方差近似表示然后做一次批量优化得到一组更一致的结果再用它们重新初始化一个全新的 ISAM2。这个思路有点类似关键帧系统和滑动窗口的周期性“global bundle adjustment”。代价是重置那一帧会比较耗时但可以把长周期运行的平均耗时控制在合理范围。5. 常见问题与排查实录5.1 更新后结果发散或出现 NaN这个是我见过最多的问题也是新手最容易踩的坑。ISAM2 是一个非常依赖初始值的增量算法它不像批量优化那样有多轮全局迭代来“挽救”一个离谱的初始值。如果新变量在initial里没有给定合适的初值或者干脆没插入update时会把默认零值当成初始估计那线性化点离真实状态可能非常远高斯牛顿的增量方向一旦算歪了数值溢出、NaN、轨迹飞掉这些情况就全来了。我的解决方案是所有新位姿变量都用上一时刻的估计值叠加相对运动来初始化。比如激光里程计给的是相邻两帧的相对位姿那我就把上一帧的全局位姿乘以这个相对变换作为当前帧的初始值。对于新路标则用当前相机位姿把观测值投影到全局坐标。总之初始值宁可给得糙一点也不要给零更不能不传。5.2 误差下降得很慢甚至不下降如果你的 ISAM2 跑起来不报错但误差就是降不下去多半是重线性化配置出了问题。我之前就吃过一次亏为了追求帧率把relinearizeThreshold调到了 0.5还把relinearizeSkip开到了 20结果系统是快了但轨迹误差越来越大越跑越偏。排查思路很简单先临时把relinearizeThreshold调到 0.001、relinearizeSkip调到 1再看误差是否恢复。如果误差立刻下降说明确实是重线性化策略太保守。另外也要检查是不是某些因子的噪声模型给得太松了比如把里程计的噪声设成 0.5 这种离谱值那优化器会觉得“差一点也无所谓”自然降不下去。5.3 系统越跑越慢单次更新耗时飙升ISAM2 的增量特性决定了绝大多数更新的耗时是平稳且低的但偶尔会出现单次耗时突然飙升。这时候第一反应应该是看是不是刚发生了回环闭合。回环因子会把两个原本距离很远的子树“焊接”在一起触发的重消元范围会非常大。这时候耗时飙升是正常的不用太担心。如果系统是“整体”越跑越慢而不是偶发那就要考虑贝叶斯树已经过度膨胀了。我前面提到的定期批量重置就是一个有效手段。还有一个辅助技巧是用ISAM2Result里返回的变量数量和信息判断每次更新到底动了多少变量。如果绝大多数更新的reeliminated数量都接近全图规模那说明你的贝叶斯树已经形同虚设赶紧做一次重置更划算。现象可能原因解决方向轨迹发散 / NaN新变量初始值缺失或太差用运动模型预测值初始化误差降不下去重线性化太保守或噪声给得过大调小阈值检查噪声模型回环后误差收敛慢relinearizeSkip 太大改成 1 或 2整体越跑越慢贝叶斯树膨胀定期批量重置ISAM2 学到这里其实已经能应对绝大多数在线 SLAM 的落地场景了。贝叶斯树的原理和 GTSAM 的参数配置只要上手调过一次后面再遇到类似问题心里就有底了。我个人在实际操作中的体会是ISAM2 最怕的不是算法本身不行而是使用者在初始值和重线性化策略上偷懒。你给它的初始值足够合理、线性化触发足够敏感它就能在毫秒级更新里给出接近批量优化的精度你一旦图省事它立刻就在精度和稳定性上报以颜色。下一篇我打算写写贝叶斯树的可视化工具和回环闭合场景里的实战对比到时候可以放一些实际的轨迹图和误差曲线出来。