视觉SLAM第10讲｜后端优化(下)：位姿图优化、滑动窗口与图优化工程实践

第9讲的全量光束法平差，虽然全局精度最高，但有个致命的工程痛点：帧越多优化变量就越多，计算量一路上涨，在线实时SLAM根本扛不住。第10讲就来拆解两个工程上更常用的后端方案：滑动窗口优化和位姿图优化，看看怎么在精度和效率之间找到最优平衡。

全量BA是把所有历史帧、所有地图点都放进来一起优化，数据量随时间线性增长，越跑越慢，只适合离线批量处理。在线实时场景，必须控制优化的规模，这就是滑动窗口优化的核心思路。

固定一个窗口大小，只优化窗口内最近的N帧相机位姿和对应的地图点。

新的关键帧进来，就把最老的一帧移出窗口。这样优化变量的数量始终稳定，计算量不会随时间无限膨胀。

固定法
移出窗口的帧直接固定位姿，不再参与优化。
优点：实现简单，计算量最小；
缺点：老帧的误差无法再修正，累积漂移会慢慢变大。
边缘化法
老帧虽然不再作为优化变量，但它携带的约束信息，会通过舒尔补折算成先验项，保留在海塞矩阵里。
优点：保留了老帧的信息，精度比固定法好很多，是工程上的主流方案。

💡通俗解释：滑动窗口就像开车看后视镜，只盯着最近的一段路，不是整条历史都看。边缘化就是老的路况虽然不持续盯着了，但它的影响还留在约束里，不是直接丢掉不管。

如果说滑动窗口是从「时间范围」上砍规模，位姿图就是从「变量类型」上砍规模。

BA里绝大多数优化变量都是地图点，相机位姿只占很小一部分。如果能把地图点全部"消掉"，只保留相机位姿，变量数量会暴跌，优化速度能快一个数量级。

用第9讲讲的舒尔补技巧，把所有地图点变量边缘化掉。原本点对相机的观测约束，会转化成相机与相机之间的相对位姿约束。

最后优化变量就只剩下各个时刻的相机位姿，这就是位姿图（Pose Graph）。

顶点：每个关键帧的相机位姿，用SE(3)表示。
边：两个位姿之间的相对运动约束。
边的观测值主要来自两个地方：
1. 前端视觉里程计输出的相邻帧相对位姿；
2. 回环检测输出的回环边，两个相距很远的帧其实是同一个位置。

位姿图优化的目标，就是最小化所有边的相对位姿误差平方和。

每个边的误差：两个顶点的位姿做差，和观测的相对位姿对比，误差转成李代数算范数平方。

💡通俗解释：全量BA就像同时调整所有路灯和所有路牌的位置，让每束光都精准对准；位姿图就是先把路牌的影响折算好，只调整路灯的位置，让路灯之间的相对位置误差最小。工作量小很多，速度提升非常明显。

一张表看懂三种后端方案的取舍：

💡工程经验：实际系统从来不是单选一种。通常是窗口内做BA保证局部精度，检测到回环后触发全局位姿图优化，拉回整体漂移。分层组合，兼顾速度和精度。

位姿图这类图结构的优化问题，最适合的工具就是g2o库，它是专门为图优化设计的通用框架，也是SLAM领域的标配工具。

💡踩坑提示：位姿图的初始值很重要。如果初始偏差太大，非线性优化容易陷入局部极小。所以通常都是先用前端VO给出初始位姿，再用图优化做精修。

位姿图最核心的应用场景就是回环校正：

这就是回环能消除漂移的核心原理：靠一条回环边的约束，把整条轨迹的误差重新分布。

第9讲和第10讲合起来，就讲完了视觉SLAM后端优化的核心体系。

全量BA精度拉满，适合离线小场景；滑动窗口BA兼顾局部精度和实时性，是在线前端的标配；位姿图轻量高效，配合回环检测做全局校正，是大场景建图的核心。

到这里，前端、后端的核心算法就都齐了。下一讲我们进入回环检测，看看怎么判断"这个地方我来过"，以及怎么用回环把漂移的轨迹拉回来。