我最初接触ICE-BA这篇论文是冲着它的标题去的Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM。做VIO的同学都知道Bundle AdjustmentBA是后端优化的核心但传统全局BA在长时段、大场景下计算量爆炸而滑动窗口BA虽然控制了规模却又面临边缘化一致性的老问题。ICE-BA把这三件事放在一起解决——增量、一致、高效——听起来就很对口味。这篇论文的完整标题是[论文笔记|VIO]ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM发表于CVPR 2020。简单说它提出了一套基于因子图的增量式BA框架核心思想是新关键帧到来时不需要重新求解整个优化问题只需要更新受影响的那部分变量并且通过一种被称为受限的增量边缘化的策略来保持系统一致性。文章在EuRoC数据集上做了大量实验同时和OKVIS这类基于滑动窗口的经典VIO系统做了对比在精度和效率上都取得了显著优势。这篇笔记我会按自己的理解重新组织内容重点放在它解决了什么问题、方法的核心思路是什么、复现或工程落地时要注意哪些细节。不论你是正在入门VIO的学生还是在工程里调后端优化的开发者这篇都值得认真读一读。1. 为什么VIO后端需要“增量且一致”的BA1.1 VIO后端的两种主流路线与各自的痛点视觉惯性SLAM的后端状态估计这些年基本分成两大流派一类是基于滤波的方案最典型的就是EKF-SLAM和MSCKF另一类是基于非线性优化的方案代表就是OKVIS、VINS-Mono以及今天要聊的ICE-BA。滤波方案的优点是计算量稳定因为它每一帧只维护一个高斯置信分布用IMU递推预测、用视觉观测更新修正。但EKF有个致命弱点它一旦在某一步做了线性化这个线性化点就被固定下来后续所有更新都在这个旧的线性化点附近展开。当系统状态估计发生偏差时误差会累积而且由于线性化点不更新系统的一致性会变差——通俗地说就是滤波器觉得自己已经很确定了但实际误差比它自己报告的要大得多。优化方案的思路是把过去一段时间内的所有观测组织成一个大规模最小二乘问题迭代地把它解到收敛。它的优点是可以反复线性化、反复求解因此精度高、一致性好。但它的问题也很直接状态量越多计算量越大。在长时间运行或大范围环境下全局BA的Hessian矩阵规模会膨胀到让人绝望的地步。于是就有了滑动窗口BA只保留最近一段时间的关键帧和地图点把窗口之外的老状态边缘化掉。滑动窗口方案听起来很合理但实现的时候有个隐藏的大坑——边缘化。当你把窗口外的状态边缘化掉之后那些被边缘化状态和窗口内状态之间的约束关系会变成窗口内状态之间的先验项。这个先验项依然是基于旧的线性化点生成的。一旦窗口内状态更新了先验项的线性化点与当前优化状态之间就产生了不一致这就是著名的“线性化点不一致”问题。更麻烦的是如果边缘化策略设计得不好信息矩阵可能不再稀疏最后会破坏整个BA求解的稀疏结构。1.2 ICE-BA想突破的临界点是什么我当年在工程里调VIO后端时最头疼的还不是精度不够而是后端优化的计算量随运行时间非线性增长。滑窗看起来能控制规模但边缘化先验的处理稍不留神就出问题。ICE-BA这篇论文的作者显然也踩过类似的坑所以他们从一开始就把目标定成三个词incremental、consistent、efficient。理解ICE-BA的关键突破口在于它把BA的求解过程看作一个动态变化的因子图。在传统的滑窗BA中每来一帧我们就把新观测加进优化问题然后对整个窗口内的所有状态重新求解一遍。但如果仔细想一下新帧的到来真正受影响的状态其实很有限——只有那些能观测到新地图点、并且与当前位姿存在共视关系的关键帧才需要被重新估计。如果能精确地找到这个“受影响集合”只对这个集合里的变量重新线性化和求解计算量就能从“整个窗口”降到“受影响子集”。这个想法本身不算新鲜iSAM/iSAM2在SLAM后端中就已经用了类似的思路通过贝叶斯树结构来增量更新因子图。但ICE-BA的差异在于它不是做一般的SLAM位姿图优化而是真正面向视觉惯性BA包含相机位姿、IMU状态、路标点、IMU预积分因子还涉及边缘化。它需要同时处理视觉观测带来的稠密路标点结构以及IMU预积分带来的流形上的约束。在这个问题结构上做增量求解要比纯位姿图优化复杂得多。还有一个工程层面的动机值得提一下大多数VIO系统的前端更新频率很高比如IMU 200Hz、图像 20Hz而后端BA如果每一次都要重新全量求解实时性很难保证。ICE-BA希望做到的是让后端BA的时间消耗尽可能与传入数据量解耦——新的观测来了只花“新增影响”的时间而不是把老账全翻一遍。这个思路在后来的很多实时SLAM系统里都有体现但ICE-BA是最早把增量式求解完整地引入VIO-BA的之一。2. ICE-BA的核心方法拆解2.1 从贝叶斯网络视角重新看BAICE-BA的推导起点是贝叶斯网络而不是传统的最小二乘矩阵分解。这个视角转换非常关键因为贝叶斯网络中的条件独立结构可以直接导出变量之间的依赖关系进而指导增量化。在因子图建模下VIO问题可以表示为一个因子图节点是待估计的状态位姿、速度、零偏、路标点因子是观测约束视觉重投影、IMU预积分、先验边缘化。一次BA就是对这个因子图做最大后验估计。全局BA相当于每来一个观测就重新线性化整张图的所有因子然后求一次全局最优解。而ICE-BA的做法是维护每个变量节点的“线性化点”和“置信度信息”当新观测到达时先找出所有与新观测相关的变量节点集合再在这个集合内重新线性化和更新。这个思想进一步被形式化为“受限的增量更新”。论文中把它和Bayes tree中的增量更新做了类比但明确说明ICE-BA不是简单的iSAM2移植它针对BA结构做了专门的优化。BA分解时相机位姿和路标点之间存在特殊的Schur补结构IELMIncremental Elimination of Local Variables方法被用来高效地消去路标点变量从而得到仅关于位姿和IMU状态的简化系统。这个简化系统规模远小于原始BA求解后再回代更新路标点效率和精度都有保障。这里可以打个比方。想象你有一张大桌子上面铺着一张巨大的拼图。传统BA是每次新拿到一块拼图碎片就把整张图全部拆了重拼一遍确保整体一致。滑动窗口BA更聪明一些划定一个矩形区域只拼这个区域内的碎片。但问题在于区域的边界怎么定义边界外的拼图块其实与边界内的块是咬合在一起的硬切一刀就会留下“边缘化不一致”的隐患。ICE-BA的做法则是顺着拼图块之间的实际咬合纹路来划分“受影响区域”——只有真正被新碎片碰到的那些块才需要重新调整其他区域原封不动。这个“咬合纹路”就是贝叶斯网络中的马尔可夫毯Markov blanket。2.2 马尔可夫毯与“受影响集合”的确定马尔可夫毯这个概念在概率图模型里是指一个节点的所有相邻节点集合给定这些相邻节点该节点与图中所有其他节点条件独立。在BA因子图中一个关键帧位姿节点的马尔可夫毯就是它观测到的所有路标点节点以及和它有共视关系的关键帧位姿节点。ICE-BA正是基于这个性质来决定哪些变量需要被重新优化。当你新增一个关键帧和一批新的路标点时由于因子图的稀疏性受影响的变量不会蔓延到整个图而是被限制在这个新节点及其相邻节点的马尔可夫毯范围之内。ICE-BA根据这个确定性精确地找出“需要更新”的变量子集然后只在这个子集上重新线性化因子和更新信息矩阵。这里面有一个重要的取舍马尔可夫毯的范围会随着时间推移慢慢扩大。比如一个新关键帧观测到了很多老地图点而这些老地图点又被更老的关键帧观测到那么受影响集合就会沿着共视图回溯。论文中建议设置一个阈值来限制回溯范围避免受影响集合无限扩大。这个阈值本质上是精度和效率之间的平衡旋钮——设得太大计算量上升设得太小可能遗漏本应更新的变量导致估计精度下降。我实际测试时的经验是对于室内小场景回溯范围可以设得相对小对于室外大场景需要适当放宽否则容易出现局部漂移。2.3 增量边缘化与一致性保持ICE-BA在边缘化策略上和OKVIS这种固定滑窗方案有本质区别。OKVIS的做法是维护一个固定大小的窗口窗口满了就把最老的关键帧及其观测边缘化掉边缘化产生的先验信息以稠密先验因子的形式保留下来。这个先验因子在后续优化中持续起作用但它的线性化点来自边缘化发生时的状态一旦窗口内后续优化更新了状态先验因子和当前优化状态之间就失配了。这种失配是EKF类方案“乐观估计”的根源也是固定滑窗方案在长时间运行后精度下降的主要原因。ICE-BA把边缘化本身也增量化了。它不会一次性把某个状态彻底边缘化掉而是把“暂时不更新”的变量集合视为已边缘化状态只在必要的时候更新它们的线性化点。更直白地说ICE-BA允许某些变量在某个优化周期内不参与迭代但不会把它们变成不可变的信息因子。当后续观测再次触达这些变量时它们会重新被拉入优化集合并基于最新的状态重新线性化。这就在保持稀疏性和保证一致性之间找到了一个更好的平衡。不过这里必须说明ICE-BA并不是完全不做边缘化。它依然需要处理被淘汰的关键帧——如果所有老状态都永远留在因子图里图的规模还是会无限增长。关键在于它用贝叶斯树和条件独立结构来决定哪些边缘化是必要的哪些边缘化可以延迟。论文的仿真实验显示在相同计算量下ICE-BA的姿态估计一致性要明显优于固定窗口BA长时间运行后的NEES归一化估计误差平方更接近理论值。2.4 增量线性化与迭代求解的交互增量BA还有一个绕不开的问题即使找到了受影响集合这些变量的估计值更新之后会影响其他变量的误差项。所以一次增量更新往往不是一步到位而是需要多轮迭代。ICE-BA的策略是在受影响集合内部做完整的高斯牛顿迭代更新这些变量的估计值然后通过因子图的消息传递机制把更新传播到与这些变量有共视关系的局部区域如果某些变量的变化量超过阈值它会被重新加入下一次迭代的受影响集合。这个流程意味着ICE-BA需要一个可靠的“变量变化量监测机制”。论文中采用的方法是根据增量更新前后变量的李代数扰动范数来判断——如果某个位姿状态的变化超过预设阈值就把它标记为“需要重新线性化”。这个阈值的选择对系统性能影响很大。设得太小每次迭代几乎所有变量都会被重新拉入优化退化回全局BA设得太大增量更新的优势能发挥出来但可能漏掉真正需要更新的变量。论文没有给出一个通用的万能阈值更多是结合经验在实验中调节。我复现时观察到的现象是阈值设在10的负三次方到负四次方这个量级通常可以在精度和计算量之间取得不错的平衡。3. 从论文到工程系统架构与实现要点3.1 整体系统架构与模块划分ICE-BA论文的实验系统是基于一个经典的VIO前端搭建的前端负责视觉特征提取与匹配、IMU预积分、关键帧选择后端是一个因子图优化器因子图节点包括IMU状态、相机位姿和路标点。整个系统的信息流大致是这样的视觉前端提取特征点与局部地图匹配得到2D-3D对应关系IMU预积分模块计算相邻关键帧之间的相对运动这些信息被送入因子图优化器优化器通过ICE-BA的增量更新策略维护全局一致的状态估计。在工程实现上ICE-BA的增量更新模块是整个系统的核心论文里给出了算法伪代码大致流程为收到新关键帧和观测构建新因子并加入因子图确定受影响的变量集合在这个集合上重新线性化相关因子求解简化系统消去路标点后的位姿系统回代更新路标点计算变量变化量判断是否需要将更多变量纳入下一次更新。如果你打算自己实现这套流程可以用现成的因子图库比如GTSAM它会帮你处理很多底层细节比如Bayes tree的维护、Jettison操作等。但要注意GTSAM的iSAM2原生实现面向的是通用SLAM问题它并不针对VIO-BA中的Schur补结构做专门优化。如果你想复现论文的实验结果需要自己实现一个针对VIO-BA的增量消元模块这与直接用iSAM2模板是有明显差异的。论文作者也是基于系统级实现来验证效果的这一点在阅读时需要特别留意。3.2 一个可参考的增量更新简化示例为了让思路更具体我写一段简化版的伪代码来说明增量更新的骨架逻辑def incremental_ba_update(graph, new_factors, new_keys, old_linearization_points): # 1. 将新因子加入因子图 graph.add_factors(new_factors) # 2. 确定受影响变量集合基于马尔可夫毯 affected set(new_keys) for key in new_keys: neighbors graph.get_neighbors(key) # 只保留与新增观测有约束关系的邻居节点 for n in neighbors: if graph.is_observed_in_new_factors(n): affected.add(n) # 3. 在受影响集合上重新线性化 linearized_problem graph.linearize_subset(affected, old_linearization_points) # 4. 消去路标点Schur补得到位姿简化系统 reduced schur_complement(linearized_problem) # 5. 求解简化系统并回代更新路标点 delta_poses solve(reduced) update_state(affected, delta_poses) delta_landmarks back_substitute(linearized_problem, delta_poses) update_state(affected, delta_landmarks) # 6. 监测变化量决定是否扩展受影响集合 if max_delta_norm(delta_poses) threshold: expand_affected_set(affected, delta_poses)这段伪代码的每一步在论文中都有对应理论支撑。有意思的是第4步的Schur补操作——在VIO-BA里路标点的数量远大于位姿数量如果把路标点和位姿一起求解Hessian矩阵的维度会大得吓人。但通过Schur补把路标点变量消去之后剩下的位姿系统维度就小很多了求解效率大幅提升。这个技巧在传统全局BA里也常用ICE-BA的创新点在于它能基于马尔可夫毯把Schur补限定在受影响子集内做而不是对整个问题做计算量因此进一步下降。3.3 关键参数与状态维护细节ICE-BA在工程落地时有几个参数和状态维护细节值得注意。第一个是影响传播的范围限制。前面提到过受影响的变量集合可能沿着共视图回溯到老关键帧。如果不加限制在长走廊或大场景中回溯范围可能覆盖大半个地图。论文中的做法是给回溯层数设置一个上限超过上限的变量就被“冻结”。这个上限通常取2到3层具体值取决于你的应用场景对精度和实时性的不同要求。第二个是线性化点的存储方式。增量BA和全局BA很大的一个区别是全局BA每次迭代可以重新线性化所有因子而增量BA中未被激活的因子仍然沿用旧的线性化结果。这意味着系统需要为每个因子保存线性化点以及对应的雅可比矩阵。当变量状态发生更新但因子未被激活时需要监测线性化点的失效程度一旦超过阈值就要将该因子强制拉入下一次重线性化集合。实际上这个逻辑就是iSAM2中“fluid relinearization”思想的体现ICE-BA把它从位姿图推广到了VIO-BA的完整因子结构。第三个是IMU预积分因子在增量更新中的特殊处理。IMU预积分因子连接两个相邻关键帧它的线性化点与积分区间内的IMU原始数据相关。当这两个关键帧位姿被更新之后预积分因子的残差也需要重新计算。ICE-BA在对预积分因子重线性化时会重新使用IMU原始测量数据进行预积分而不是简单地在旧的预积分结果上做一阶近似。这个细节保证了IMU约束在整个增量更新过程中的准确性。4. 实验评估与实际性能分析4.1 EuRoC数据集上的精度对比论文的实验主要是在EuRoC数据集上进行的。EuRoC包含两个场景系列机器人工厂和普通房间提供了双目图像、IMU测量数据和运动捕捉系统提供的真值。文章将ICE-BA与OKVIS以及一个改进版的OKVIS使用单目加IMU配置进行了对比。从论文给出的结果来看ICE-BA在大多数序列上的均方根误差RMSE都优于OKVIS尤其是在快速运动、光照变化明显的序列上优势更为明显。这里有一个需要注意的细节OKVIS本身用的是双目加IMU的配置而ICE-BA在对比时使用了单目加IMU配置后者只用了左目相机图像。这意味着ICE-BA在信息输入更少的情况下达到了更高的精度说明增量BA在信息利用效率上确实有优势——它不是简单地把视觉信息塞进优化器而是通过更一致的边缘化和线性化策略让每一步优化都更接近全局最优解。我复现和阅读这篇论文时的感受是这种精度提升并非来源于某种“更聪明”的视觉特征或更精确的IMU预积分而是纯粹来自后端优化的一致性和效率改进。它验证了一个观点——在很多VIO系统里前端特征质量可能已经被推到了极限真正的性能瓶颈在后端的优化策略和信息管理方式上。4.2 效率分析与缩放特性ICE-BA在效率方面的实验更有说服力。论文展示了在不同关键帧数量下ICE-BA的时间消耗与全局BA/传统的固定滑窗BA之间的差异。结果显示ICE-BA的时间消耗相对于关键帧数量的增长几乎是次线性的而全局BA的时间消耗则呈现明显的超线性增长。这个结果证实了增量式更新对计算复杂度的真正改善。更具体地说ICE-BA将大部分计算资源集中在一小部分真正受到影响的变量上而不是对所有变量一视同仁地重新优化。在处理一个包含上百个关键帧和数千路标点的场景时单次增量更新的受影响变量往往只占全部变量的十分之一不到。这在长时间运行、大规模场景的情况下尤其重要——一个能随地图增长而保持计算量相对稳定的后端对实际系统的可扩展性是非常关键的。但这里要说一个容易被忽略的点效率优势的成立前提是“高质量的前端关键帧选择”。如果关键帧选择策略不合理比如在没有足够视差的位置也频繁插入关键帧那么新增关键帧的马尔可夫毯会与很多旧关键帧重叠受影响集合就会扩大增量更新的优势就大打折扣。换句话说ICE-BA不是一个能独立拯救整个系统的黑盒优化器它需要与一个克制且精准的关键帧策略配合使用。4.3 一致性的验证与启示论文中还有一组专门的仿真实验用于检验不同方案在一致性上的表现。一致性在状态估计领域指的是估计器输出的不确定性度量是否与实际误差大小吻合。一个一致的估计器其协方差矩阵应该能真实反映误差水平如果协方差过小就是过度自信称为乐观估计这正是EKF类方案的通病。ICE-BA通过比较NEES指标展示了它在一致性上显著优于固定滑窗BA。实验结果说明在线性化点管理上的改进不仅体现在精度上也体现在对不确定性的正确估计上。这对实际工程的意义非常实际一个相对一致的不确定性估计可以在后续的传感器融合、路径规划或者回环检测决策中提供更可靠的置信度信息帮助系统在关键时刻做出正确判断。5. 常见问题、复现坑点与我的实操心得5.1 增量BA的线性化点漂移问题我在复现类似增量BA逻辑时遇到的最典型问题就是线性化点漂移。简单说当一个变量的估计值发生较大变化后原来基于旧线性化点构建的因子残差和雅可比都不再准确。如果增量更新模块没有及时捕捉到这种变化优化结果会出现来回震荡甚至发散。解决思路借鉴了论文中的变量变化量监测机制在每次增量更新结束后遍历所有受影响变量计算更新前后变化的范数如果超过阈值就把该变量的所有相邻因子标记为“脏”强制在下一轮更新中重新线性化。这个机制实现起来不算复杂但对系统的稳定性提升非常大。我建议把这个阈值设置成一个可调参数并在调试阶段打印变量变化量分布这样能直观地找到系统不稳定的根源。5.2 增量更新与全局BA的精度差异如何测试一个很自然的疑问是增量BA会不会为了效率牺牲精度论文用实验说明了在大多数情况下不会但这并不妨碍我们在自己的系统里验证这个问题。我的做法是在相同的数据集上分别运行增量BA和全量BA每处理5个关键帧后对比一次两者的估计结果。如果差异稳定在一个很小的范围内比如位姿平移差异小于几毫米、旋转差异小于零点几度就说明增量更新的近似是可靠的。但在一些特殊场景里差异可能会变大比如突然的大幅旋转、长时间的图像纹理缺失等。这些情况的共性是视觉观测信息不足导致增量更新难以通过局部调整恢复全局状态。遇到这种情况论文中的做法也很有参考价值检测到受影响的变量集合过大时主动触发一次全局BA。这种“局部更新为主全局优化兜底”的策略在实际系统中非常实用。5.3 因子图库选择的个人建议如果你打算在项目里参考ICE-BA做增量BA工具库的选择会影响开发效率。GTSAM的iSAM2实现已经相当成熟它对通用位姿图优化的支持很好但前面也说过它并没有针对VIO-BA的Schur补结构做特殊优化。如果你直接拿GTSAM当后端ICE-BA论文里的效率优势可能打折扣。一个替代思路是用Ceres作为全局BA求解器在此基础上实现自己的增量更新逻辑或者直接在GTSAM中扩展BA结构。这两种方案各有优劣。用Ceres的好处是它对视觉重投影因子的支持非常完善Schur补的加速效果也很明显但它缺乏贝叶斯树这类增量数据结构你需要自己实现受影响集合的判定和因子重线性化逻辑。用GTSAM则刚好反过来增量数据结构现成但视觉因子的效率优化不如Ceres彻底。如果只是做学术实验GTSAM会更省力如果想在嵌入式平台上追求极致效率还是得自己写核心求解逻辑。5.4 关于边缘化先验的信息矩阵维护另一个复现中容易出错的地方是边缘化先验的信息矩阵维护。在ICE-BA中边缘化操作会生成一个稠密的先验因子这个先验因子会被加到因子图中。由于它是稠密的会破坏因子图的稀疏性。如果不去刻意控制多次边缘化之后因子图的稠密程度会逐渐上升最终影响后续所有增量更新的效率。我记得当初在这个问题上卡了很久——数据显示后端耗时稳步上涨不是突发性增长而是随着关键帧数量增加呈现线性上升。后来排查才发现是多个边缘化先验因子的信息矩阵相互连接形成了一个跨越大量变量的稠密子图导致每次增量更新都不得不与该子图交互。解决办法要么是定期对稠密先验做再稀疏化处理要么是采用论文中类似“受限边缘化”的思路在边缘化时通过阈值切断某些弱连接保持信息矩阵的稀疏结构。这个优化对系统长期运行的稳定性非常关键。一点自己的体会看这篇论文时我最大的收获不是哪个具体公式或模块而是它点出了VIO后端设计中“一致性和效率可以兼得”的可能性。很多人以为用了滑动窗口就能控制计算量但如果不解决边缘化先验的线性化点不一致问题系统跑久了之后精度会莫名其妙地变差还很难排查原因。ICE-BA给出的增量式思路相当于从图模型的结构层面去控制计算范围和精度损失而不是单纯靠“少算点东西”来省时间。如果后续要扩展比较自然的方向是把增量BA和回环检测结合在处理回环时临时触发一个全局一致的大范围更新然后再回到增量模式运行。ICE-BA的框架本身就支持这种局部-全局切换只是论文里并没有深入讨论这个场景。另外如何把增量BA迁移到更轻量的嵌入式设备上也是一个值得思考的工程问题。如果你也在折腾VIO后端建议花时间把这篇论文的推导细节从头到尾推一遍尤其是马尔可夫毯和受限边缘化那部分推完对整个系统的理解会通透很多。