位姿图优化：从相对约束到全局轨迹

1. 边的信息矩阵设错了，比没有回环还糟

位姿图优化（PGO）是我第一次把 SLAM 从「前端跟踪」拉到「全局一致性」的地方。前端给相对位姿边；回环偶尔插入长程边；后端找一组位姿让整体张力最小。真机里最难的是：信息矩阵设错了，比没有回环还糟——一次误回环 + 硬边，地图叠成两层。

2. 问题形式

关键帧位姿 $x_{i} \in SE (3)$ 。里程计给相邻帧相对变换 $T^_{ij}$ ，回环给 $(a, b)$ 之间的 $T^_{ab}$ 。PGO 最小化：

min (i, j) \in E \sum ρ (Log (T^ij^{- 1} T_{i}^{- 1} T_{j}) Ω_{ij}^{2})

$Log$ 把 SE(3) 误差映到切空间 $R^{6}$ ； $Ω_{ij} = Σ_{ij}^{- 1}$ 是信息矩阵（边的硬度）。PGO 不优化路标，只动位姿——比完整 BA 快一个数量级，也是精度上限的来源。

3. 信息矩阵：不是越大越好

边的协方差 $Σ_{ij}$ 来自前端匹配质量：内点数、重投影 RMSE、ICP fitness、退化方向不确定性。

做法	后果
所有里程计边同等权重	转弯处匹配差仍被当硬约束，轨迹局部扭曲
回环边信息矩阵过大	一次误回环把整条轨迹拧成麻花
忽略退化方向	走廊里沿隧道方向的边几乎无信息，却被当硬约束

我的习惯：里程计边按局部匹配分数动态缩放；回环边先用保守信息，Chi2 检验通过后再逐步收紧。ORB-SLAM 系回环后先做 PGO 再可选 Global BA——先粗调全局，再精修局部几何。

4. 与完整 BA 的分工

Bundle Adjustment 同时优化位姿与路标；PGO 只动位姿，假设地图点/子地图已固定。流水线：

text

回环检测 → 几何验证 → PGO（全局位姿） → 可选 Local/Global BA（地图点）

LiDAR SLAM 里子地图锚定 + PGO 更常见：每个 submap 内部已做过 scan-to-map 优化，跨 submap 只估相对位姿。Cartographer、LIO-SAM 的后端本质都是位姿图，边的来源不同（scan matching vs IMU 预积分）。

5. 求解与失败模式

位姿图 Hessian 极度稀疏：每个因子只连两个位姿节点。g2o、GTSAM、Ceres 都能高效求解。关键细节：流形线性化扰动用右乘 $T \oplus ξ = T \cdot Exp (ξ)$ ；回环边加 Huber/Cauchy 抑制外点；固定一个位姿锚定 gauge freedom。Cholesky 失败往往意味着图里有矛盾约束——优先查误回环，不是盲目加迭代次数。

误回环 + 硬边：仓库对称工位，词袋召回相似帧，RANSAC「幸运」通过，PGO 后地图叠两层。教训：回环边宁软勿硬，必须有 Chi2 门控和 switchable constraints。

里程计尺度漂移：轮速标定偏 3%，PGO 只能把形状拉直，米制尺度仍错——不可观自由度在 PGO 里照样不可观。

频繁 PGO 抖动导航：每来一个回环就全图优化，TF 跳变导致 Nav2 路径重规划风暴。产品里常做：PGO 在后台线程，输出平滑后的 map→odom 修正。

2D Cartographer 的 optimize_every_n_nodes 和 LIO-SAM 的 ISAM2 update 本质都是「何时触发 PGO」。调得太频 CPU 飙；调太稀回环修正滞后。现场宁可 PGO 慢一点，也要把每条回环边的 Chi2 和修正量打 log，方便事后删边。

6. Switchable constraints 与删边

误回环接入后，硬删边比重新跑全图便宜。g2o 的 switchable edge、Ceres 的 loss function 让优化器自己降权外点边——但 product 里我仍保留人工 audit log：Chi2 超阈值、修正量 >30 cm 的边标红，便于 offline 删边重跑。一次 PGO 把 200 帧修正 >50 cm，几乎必是误回环，不是「优化不够迭代」。

7. 与 SLAM 前端的接口

PGO 输入边的 pose 应是 keyframe 坐标系下的相对变换，且与前端 covariance 一致。前端给 SE(3) 但协方差只填对角 6 维同值，退化方向信息丢失——PGO 会把走廊方向的软约束当硬边。从 Hessian 特征分解导出边的方向性 uncertainty，比统一填 1e-4 更 honest。

8. 验收

PGO 前后 ATE/RPE 变化（对齐方式一致）
每条回环边的 Chi2 与内点率
优化迭代：残差下降曲线、最大位姿修正量（单帧 >30 cm 要警惕）
重叠区域点云/地图一致性（视觉看重投影，激光看 ICP 残差分布）
PGO 触发频率 vs CPU 占用
TF 跳变幅度与 Nav2 重规划次数
误回环边的 switchable weight 收敛值（应接近 0）

PGO 不创造信息，只重分配已有约束。边的质量与硬度是后端契约——前端给垃圾边，PGO 只会把垃圾拉成全局一致的形状。