我最初接触ICE-BA这篇论文，是冲着它的标题去的：Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM。做VIO的同学都知道，Bundle Adjustment（BA）是后端优化的核心，但传统全局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后端需要“增量且一致”的BA
1.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 从贝叶斯网络视角重新看BA
ICE-BA的推导起点是贝叶斯网络，而不是传统的最小二乘矩阵分解。这个视角转换非常关键，因为贝叶斯网络中的条件独立结构可以直接导出变量之间的依赖关系，进而指导增量化。
在因子图建模下，VIO问题可以表示为一个因子图：节点是待估计的状态（位姿、速度、零偏、路标点），因子是观测约束（视觉重投影、IMU预积分、先验边缘化）。一次BA就是对这个因子图做最大后验估计。全局BA相当于每来一个观测就重新线性化整张图的所有因子，然后求一次全局最优解。而ICE-BA的做法是：维护每个变量节点的“线性化点”和“置信度信息”，当新观测到达时，先找出所有与新观测相关的变量节点集合，再在这个集合内重新线性化和更新。
这个思想进一步被形式化为“受限的增量更新”。论文中把它和Bayes tree中的增量更新做了类比，但明确说明ICE-BA不是简单的iSAM2移植，它针对BA结构做了专门的优化。BA分解时，相机位姿和路标点之间存在特殊的Schur补结构，IELM（Incremental 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后端，建议花时间把这篇论文的推导细节从头到尾推一遍，尤其是马尔可夫毯和受限边缘化那部分，推完对整个系统的理解会通透很多。
