位姿图优化

**位姿图优化（Pose graph optimization, PGO）**是SLAM后端的一种简化形式，其变量只有机器人/相机位姿 $\{T_i\} \subset SE(3)$——不包含三维地图点。该图包含：

• 节点：位姿 $T_i$（通常是关键帧位姿）。
• 边：带不确定度 $\Sigma_{ij}$ 的相对位姿约束 $T_{ij}$，来源于里程计（连续位姿之间）和回环检测（被识别为同一地点的非连续位姿之间）。

每条边表达的意思是”从节点 $i$ 出发，节点 $j$ 应出现在相对位姿 $T_{ij}$ 处”。优化的目标是找到能同时最好地满足所有约束的位姿配置：

$$\min_{\{T_i\}} \sum_{(i,j) \in \mathcal{E}} \left\| \log\!\left(T_{ij}^{-1}\, T_i^{-1} T_j\right) \right\|^2_{\Sigma_{ij}^{-1}}$$

残差通过 $SE(3)$ 的对数映射计算，因此误差存在于6维自由度的切空间中；该问题用高斯-牛顿法或列文伯格-马夸特法在流形上求解，与光束法平差完全相同，只是变量数量少得多。

视觉SLAM系统中典型的使用模式：

1. 前端跟踪相机并构建里程计边；漂移不断累积。
2. 场景识别检测到一次回环，几何验证在两个时间上相距较远的关键帧之间生成一条相对位姿边。
3. PGO将累积的漂移重新分配到整条轨迹上，把回环”闭合”起来。
4. 之后可选地用一次完整的光束法平差进一步细化位姿与地图点。

由于PGO舍弃了（大量的）地图点变量，它比完整的光束法平差快得多，是回环检测之后进行全局轨迹修正的标准工具——这正是ORB-SLAM的本质图（essential-graph）优化和大多数LiDAR SLAM后端（例如通过GTSAM或g2o实现）所运行的内容。在单目SLAM中，该图通常在 $\mathrm{Sim}(3)$ 而非 $SE(3)$ 上优化，以便同时纠正尺度漂移。

需要注意的一点：PGO信任它的每一条边。一条错误的回环边就足以使整个地图自我折叠，这也是促使人们研究鲁棒位姿图优化技术的原因。

优化如何运行

PGO是流形上的非线性最小二乘问题。旋转量不能通过简单加法更新，因此每一次高斯-牛顿/LM迭代都在切空间中进行：

1. 对每条边，计算残差 $\mathbf{e}_{ij} = \log\!\left(T_{ij}^{-1} T_i^{-1} T_j\right) \in \mathbb{R}^6$ 以及它关于两个位姿的微小扰动 $\boldsymbol{\delta}_i, \boldsymbol{\delta}_j$ 的雅可比矩阵。
2. 组装并求解正则方程 $H \boldsymbol{\delta} = -\mathbf{b}$，其中 $H = \sum J_{ij}^T \Sigma_{ij}^{-1} J_{ij}$ 是稀疏的：每条边只涉及两个位姿，因此 $H$ 的稀疏模式就是图的邻接结构。
3. 用收缩映射（retraction）更新每个位姿 $T_i \leftarrow T_i \cdot \mathrm{Exp}(\boldsymbol{\delta}_i)$，并迭代直至收敛。

有两个结构性细节很重要。第一，规范自由度（gauge freedom）：将所有位姿一起刚性移动不会改变代价，因此在锚定规范之前 $H$ 是奇异的——需要固定第一个位姿（或对其添加先验因子）。第二，信息矩阵 $\Sigma_{ij}^{-1}$ 是决定如何分配修正量的关键旋钮：可信度高的里程计边只会发生很小的弯曲,不确定的边则吸收大部分回环误差。将所有边都设为单位信息矩阵”可以运行”，但会以不符合实际的方式分配漂移。

一个直观的例子

想象一个机器人沿着一个大正方形行驶,里程计在每个拐角处都累积一点航向误差,因此估计出的终点位姿明显偏离起点。场景识别随后将最后一帧视图与第一帧匹配,增加了一条回环边,表明”这两个位姿是同一个地方”。在PGO之前,这条边的残差非常大,而里程计边的残差几乎为零。优化器找到的配置会分摊误差：每个位姿都会略微旋转和平移(按其边的协方差加权),每条里程计边承担一小部分残差,而回环边的残差则收缩到相同的统计尺度——轨迹会明显”合拢”成一个闭合的正方形。没有进行任何新的测量;只是把同样的测量重新一致地解释了一遍。这种”误差重新分配”的图景,正是理解每一个基于图的SLAM后端的正确心智模型。

常见陷阱

• 忘记固定规范 ——奇异的 $H$ 会表现为求解器失败,或整个地图在迭代之间自由漂移。
• 输入未经验证的回环边 ——在一条边进入图之前,务必进行几何验证(内点数量、一致性检查);一条离群边就可能造成灾难性后果(参见鲁棒PGO)。
• 大回环下初始化不佳 ——高斯-牛顿只能局部收敛;若轨迹已漂移了90°的航向,可能需要鲁棒核函数、更好的初始猜测(例如旋转平均/生成树初始化),或带阻尼的LM以避免陷入不良的局部极小值。
• 二维中的角度处理 ——将 $\theta$ 残差包裹到 $[-\pi, \pi]$ 区间内;未做角度回绕是二维位姿图”爆炸”的经典原因。
• 在需要 $\mathrm{Sim}(3)$ 的场合使用 $SE(3)$ ——单目回环检测还必须纠正尺度漂移;在 $SE(3)$ 上优化会在回环处留下尺度接缝。

对SLAM的意义

PGO是回环检测的主力工具——正是这一步将存在漂移的里程计轨迹转变为全局一致的地图。它是基于图的SLAM最简单的实例,因此也是在深入研究完整的光束法平差和因子图之前,理解流形上非线性最小二乘、稀疏性和信息矩阵的最佳起点。

动手实践

• Kimera-RPGO 动手实践

相关条目

• 因子图
• 鲁棒位姿图优化
• 李群
• 作为稀疏非线性最小二乘问题的MAP推断