Global Optimization

一、全局优化的目标与核心思想

在视觉惯性SLAM (Simultaneous Localization and Mapping) 系统中，全局优化 (Global Bundle Adjustment, GBA) 是一个至关重要的后端优化步骤。其核心目标是优化系统中所有关键帧的位姿（位置和姿态）以及所有三维地图点（Map Points）的空间坐标。通过这一过程，可以显著减少由于传感器噪声、运动估计误差和匹配不确定性等因素造成的累积漂移，从而构建一个全局一致且高精度的地图，并提升相机自身的定位精度。

核心思想：捆绑调整 (Bundle Adjustment, BA)

全局优化的数学实质是执行一次大规模的捆绑调整。这个名字形象地描述了其工作原理：想象场景中的三维点通过光线（bundles of rays）投影到不同时刻相机捕获的图像上。BA的目标就是通过微调每一个关键帧的位姿和每一个三维地图点的位置，使得这些三维点在各个观测到它的关键帧图像上的重投影误差 (Reprojection Error) 之和最小。重投影误差指的是观测到的图像特征点位置与根据当前估计的相机位姿和三维点位置计算得到的理论投影位置之间的差异。

与局部建图线程中进行的局部捆绑调整 (Local Bundle Adjustment) 相比，全局优化考虑的是整个SLAM过程中积累下来的所有关键帧和地图点，因此计算量更大，但对全局一致性的提升也更为显著。

二、全局优化的关键元素：顶点与边（基于图优化框架 G2O）

全局优化问题通常被构建为一个图（Graph），其中节点（顶点）代表待优化的变量，边代表这些变量之间的约束关系。开源图优化库g2o是ORB-SLAM2中实现BA的核心工具。

1. 顶点 (Vertices)

顶点代表了需要优化的参数。在全局BA中，主要有两类顶点：

  • 关键帧位姿 (KeyFrame Poses):
    • 表示相机在采集每个关键帧时相对于世界坐标系的六自由度位姿（3个平移，3个旋转）。
    • 在g2o中，通常使用 g2o::VertexSE3Expmap 类型来表示。SE(3)代表三维刚体变换（旋转和平移），Expmap是李代数到李群的一种映射方式。
    • 参考基准: 为了避免整个系统的位姿发生整体漂移（即所谓的“无约束自由度”），必须固定一个参考。通常，系统中的第一个关键帧（索引为0的关键帧）的位姿被设为固定 (fixed)，不参与优化。所有其他关键帧的位姿和地图点都将相对于这个固定的参考系进行调整。
  • 地图点 (Map Points):
    • 表示场景中稳定的三维特征点的世界坐标 (x, y, z)。
    • 在g2o中，通常使用 g2o::VertexSBAPointXYZ 类型来表示。SBA代表稀疏捆绑调整 (Sparse Bundle Adjustment)。
    • 这些顶点代表了地图的几何结构。

2. 边 (Edges)

边代表了顶点之间的约束关系，即观测模型。在全局BA中，主要的边是连接地图点和观测到该地图点的关键帧之间的投影关系。

  • 二元边 (Binary Edges): 因为一条边连接了两个顶点（一个地图点顶点和一个关键帧位姿顶点）。
  • 误差定义: 边的核心是定义一个误差项，即前面提到的重投影误差。
  • 边的类型根据相机模型而不同:
    • 单目相机 (Monocular Camera):
      • 使用 g2o::EdgeSE3ProjectXYZ 类型的边。
      • 这种边约束的是一个三维地图点在给定相机位姿和相机内参的情况下，投影到该关键帧图像上的二维像素坐标。误差是二维向量 (eu,ev)(e_u, e_v)。
    • 双目相机 (Stereo Camera) 或 RGB-D 相机:
      • 使用 g2o::EdgeStereoSE3ProjectXYZ 类型的边。
      • 对于双目相机，除了二维像素坐标 (uL,vL)(u_L, v_L) 外，还会利用左右目图像的视差信息来约束点的深度，或者直接约束其在右图像中的 uRu_R 坐标。因此，误差通常是三维的 (euL,evL,euR)(e_{uL}, e_{vL}, e_{uR}) 或 (euL,evL,edepth)(e_{uL}, e_{vL}, e_{depth})。
      • 对于RGB-D相机，可以直接获取深度信息，因此约束也是三维的。

书中强调，全局优化中顶点和边的类型定义与局部地图优化函数 (LocalBundleAdjustment) 中的是相同的，误差类型也一致。主要的区别在于全局优化作用于全局地图，而局部优化作用于局部地图。

三、全局优化的详细流程

ORB-SLAM2中的全局优化函数 GlobalBundleAdjustemnt (在 Optimizer::BundleAdjustment 函数中实现主体逻辑) 主要遵循以下步骤：

第1步：初始化g2o优化器

这是进行图优化的准备工作，配置优化求解的框架。

  • 创建稀疏优化器对象:
    • 例如 g2o::SparseOptimizer optimizer;
  • 选择并配置求解器 (Solver):
    • 优化问题通常是非线性的最小二乘问题。求解这类问题需要迭代计算，每一步迭代通常涉及求解一个线性方程组 HΔx=−bH \Delta x = -b。
    • 线性求解器 (Linear Solver): 负责求解这个线性方程组。常见的选择是基于Cholesky分解或QR分解的求解器，如 g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>()，这里使用了Eigen库进行矩阵运算。
    • 块求解器 (Block Solver): 封装了线性求解器，并定义了H矩阵的块结构。这里的 g2o::BlockSolver_6_3 表示H矩阵中，与相机位姿相关的块是 6×66 \times 6（SE3位姿有6个自由度），与地图点相关的块是 3×33 \times 3（三维点有3个自由度）。
  • 选择优化算法 (Optimization Algorithm):
    • 指定了如何进行迭代下降。常用的有高斯-牛顿法 (Gauss-Newton) 或莱文贝格-马夸特法 (Levenberg-Marquardt, LM)。
    • 书中代码示例使用了 g2o::OptimizationAlgorithmLevenberg。LM算法结合了高斯-牛顿法和梯度下降法的优点，具有较好的鲁棒性。
  • 设置优化器算法:
    • optimizer.setAlgorithm(solver);
  • 外部停止标志 (Optional):
    • optimizer.setForceStopFlag(pbStopFlag); 允许从外部提前终止优化过程。

第2步：向优化器中添加顶点

将所有待优化的关键帧位姿和地图点作为顶点加入到优化器中。

  • Step 2.1: 添加关键帧位姿作为顶点:
    • 遍历当前地图中所有的有效关键帧 ( vpKFs )。
    • 对每个未标记为 isBad() 的关键帧 pKF：
      • 创建一个 g2o::VertexSE3Expmap 对象: vSE3 = new g2o::VertexSE3Expmap();
      • 设置初始估计值: 将关键帧当前的位姿 pKF->GetPose() 转换为g2o的SE3Quat格式，并设置为顶点的初始估计 vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));。
      • 设置顶点ID: vSE3->setId(pKF->mnId); 使用关键帧自身的ID作为顶点的ID。
      • 固定参考帧: 如果是第0帧 (pKF->mnId == 0)，则将其固定 vSE3->setFixed(true);。
      • 将顶点添加到优化器: optimizer.addVertex(vSE3);
      • 记录已添加的关键帧的最大ID maxKFid，用于后续地图点顶点ID的设置。
  • Step 2.2: 添加地图点作为顶点:
    • 遍历当前地图中所有的有效地图点 ( vpMP )。
    • 对每个未标记为 isBad() 的地图点 pMP：
      • 创建一个 g2o::VertexSBAPointXYZ 对象: vPoint = new g2o::VertexSBAPointXYZ();
      • 设置初始估计值: 将地图点当前的世界坐标 pMP->GetWorldPos() 转换为g2o的Vector3d格式，并设置为顶点的初始估计 vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));。
      • 设置顶点ID: 为了避免与关键帧顶点的ID冲突，地图点顶点的ID通常设置为 pMP->mnId + maxKFid + 1;。
      • 边缘化/舒尔消元 (Marginalization): vPoint->setMarginalized(true); 这是一个非常重要的步骤。在BA问题中，地图点的数量通常远大于相机位姿的数量。通过将地图点顶点设置为边缘化，可以在求解线性方程组时，利用舒尔消元的技巧，先消去与地图点相关的变量，从而显著减小需求解的线性系统的规模，提高求解效率。这要求H矩阵具有特定的稀疏结构（相机-点分离）。
      • 将顶点添加到优化器: optimizer.addVertex(vPoint);

第3步：向优化器中添加投影边

在添加地图点顶点的同时，遍历该地图点被哪些关键帧观测到，并为每一次有效的观测添加一条投影边。

  • 对于每个地图点 pMP，获取其观测信息 observations = pMP->GetObservations()，这是一个记录了观测到此点的关键帧及其在该关键帧中对应特征点索引的map。
  • 遍历 observations 中的每一个观测 (pKF, feature_index)：
    • 检查观测关键帧 pKF 是否有效 (非 isBad() 且 pKF->mnId <= maxKFid，确保是已加入优化器的关键帧)。
    • 获取该地图点在当前关键帧 pKF 图像上的二维特征点（通常是去畸变后的归一化坐标或像素坐标） kpUn = pKF->mvKeysUn[feature_index];。
    • 根据相机类型创建边:
      • 单目相机模式 (pKF->mvuRight[feature_index] < 0):
        • 创建 g2o::EdgeSE3ProjectXYZ 对象: e = new g2o::EdgeSE3ProjectXYZ();
        • 连接顶点:
          • 边连接的第0个顶点是地图点: e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id))); ( id 是地图点顶点的ID)。
          • 边连接的第1个顶点是关键帧位姿: e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));。
        • 设置测量值 (Observation/Measurement): obs << kpUn.pt.x, kpUn.pt.y; e->setMeasurement(obs); 测量值即观测到的二维特征点坐标。
        • 设置信息矩阵 (Information Matrix): 信息矩阵是测量噪声协方差矩阵的逆，它决定了这条边在总误差函数中的权重。通常与特征点在图像金字塔中的层级 kpUn.octave 相关，层级越高（图像越小，特征点越模糊），其不确定性越大，信息值（权重）越小。 const float& invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
        • 设置鲁棒核函数 (Robust Kernel): 为了减小外点（错误的观测）对优化结果的负面影响，会使用鲁棒核函数。当误差（重投影误差）较大时，鲁棒核函数会动态降低该误差项的权重。
          • g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk);
          • rk->setDelta(thHuber2D); Delta是Huber核的阈值，例如 thHuber2D = sqrt(5.99) (对应卡方分布自由度为2，置信度95%的阈值)。如果误差的平方超过这个阈值，其对总代价的贡献会从二次变为线性，避免误差过大导致优化跑飞。
        • 设置相机内参: 投影计算需要相机内参 (焦距fx, fy, 主点cx, cy)。 e->fx = pKF->fx; e->fy = pKF->fy; e->cx = pKF->cx; e->cy = pKF->cy;
        • 将边添加到优化器: optimizer.addEdge(e);
      • 双目相机或RGB-D相机模式 (书中未详述，但原理类似):
        • 会创建 g2o::EdgeStereoSE3ProjectXYZ 类型的边。
        • 测量值会包含深度信息或右图像的u坐标。例如，对于双目，测量值可能是 (uL,vL,uR)(u_L, v_L, u_R)。
        • 信息矩阵通常是 3×33 \times 3 的。
        • 鲁棒核的阈值可能是 thHuber3D = sqrt(7.815) (对应卡方分布自由度为3，置信度95%的阈值)。
        • 同样需要设置相机内参，包括双目基线 bf。

第4步：开始优化

配置完成后，启动优化迭代过程。

  • 初始化优化: optimizer.initializeOptimization(); 这一步会构建Hessian矩阵的结构等。
  • 执行优化: optimizer.optimize(nIterations); nIterations 是预设的迭代次数（例如书中提到的10次）。优化器会使用选择的算法（如LM）迭代更新顶点的估计值，直到满足收敛条件或达到最大迭代次数。

第5步：将优化的结果保存起来

优化完成后，优化器中的顶点值已经更新为最优估计。需要将这些结果提取出来并更新到SLAM系统的关键帧和地图点对象中。

  • 更新关键帧位姿:
    • 遍历所有参与优化的关键帧 vpKFs。
    • 对于每个有效关键帧 pKF:
      • 从优化器中获取对应的位姿顶点: g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
      • 获取优化后的位姿估计: g2o::SE3Quat SE3quat = vSE3->estimate();
      • 有条件地更新位姿:
        • 特殊情况 (nLoopKF == 0): 书中提到，这通常表示该全局BA是在系统刚初始化，地图中只有少量关键帧时（例如，创建初始地图点时）调用的。此时，优化后的位姿可以直接写入关键帧的成员变量中: pKF->SetPose(Converter::toCvMat(SE3quat));
        • 常规情况 (闭环后调用): 优化后的位姿会先存储在关键帧的一个专门的成员变量 mTcwGBA 中，而不是立即覆盖原始位姿。 Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA); 这样做可能是为了后续进行验证、或者等待其他模块（如位姿图优化）确认后再统一更新，以保证系统状态的一致性。同时记录是哪个闭环触发了这次全局BA pKF->mnBAGlobalForKF = nLoopKF;。
  • 更新地图点位置: (书中代码片段未完整展示这部分，但逻辑相似)
    • 遍历所有参与优化的地图点 vpMP。
    • 对于每个有效地图点 pMP:
      • 从优化器中获取对应的地图点顶点 (使用 pMP->mnId + maxKFid + 1 作为ID)。
      • 获取优化后的三维坐标估计。
      • 将优化后的坐标更新回地图点对象 pMP->SetWorldPos(...)。
      • 如果一个地图点在优化后其观测边数量过少，或者误差过大，可能会被判断为外点并剔除。

四、全局优化的意义与作用

  1. 消除累积误差: SLAM系统在长时间运行时，由于传感器测量噪声、特征匹配的不确定性以及运动估计的微小误差会不断累积，导致估计的轨迹和地图逐渐偏离真实情况。全局优化通过同时考虑所有历史观测和约束，将这些累积误差在整个系统中进行分散和最小化。
  2. 提高地图一致性: 全局优化确保了构建的地图在全局尺度上是几何一致和准确的。这对于大范围场景的建图尤为重要。特别是在发生回环检测 (Loop Closure) 后（即相机回到了先前经过的区域并成功识别出来），全局优化是修正整个轨迹和地图，使回环处的轨迹闭合、地图对齐的关键步骤。
  3. 提升定位精度: 通过优化所有关键帧的位姿，使得相机在地图中的定位更加准确可靠。一个经过良好优化的地图是后续高精度定位的基础。

全局优化是计算密集型操作，因此在SLAM系统中不会频繁执行，通常在检测到显著的回环或者累积误差达到一定程度时触发。它是保证SLAM系统长期稳定性和准确性的核心模块之一。