局部建图线程中局部地图优化 (Local Bundle Adjustment in Local Mapping Thread)

引言

在 ORB-SLAM2 系统中，局部建图线程负责处理新的关键帧，并优化局部的地图。LocalBundleAdjustment (局部BA) 是该线程中的核心函数，主要用于优化当前关键帧及其邻近区域的关键帧位姿和它们观测到的地图点三维位置，以构建一个局部一致的地图。

核心思想

全局的 Bundle Adjustment (BA) 会优化所有的关键帧和地图点，计算成本非常高。局部BA的思想是只对当前活跃的、与当前关键帧紧密相关的一个子图进行优化。这大幅降低了计算复杂度，使得系统能够实时运行，同时通过不断优化局部区域，逐步累积实现全局地图的一致性。

关键概念 (参考图 14-2)

理解局部BA，首先要明确参与优化的不同元素：

当前关键帧 (Current KF):
- 定义：最新插入到地图的关键帧，是本次局部BA的触发者和中心。
- 图14-2标识：中间的红色相机图标 (“当前KF”)。
局部关键帧 (LocalKeyFrames / 待优化局部关键帧):
- 定义：这些是位姿将会被优化的关键帧集合。
- 组成：
  1. 当前关键帧 (Current KF)。
  2. 一级相连关键帧 (First-level connected KeyFrames): 与当前关键帧有共同观测地图点的关键帧（即共视关键帧）。它们是当前关键帧的直接邻居。
- 图14-2标识：所有红色相机图标（“当前KF”，“KF的共视关键帧KF1”，“KF的共视关键帧KF2”）。这些构成了 ILocalKeyFrames。
局部地图点 (LocalMapPoints / 待优化局部地图点):
- 定义：这些是三维位置将会被优化的地图点集合。
- 组成：所有被“局部关键帧”集合中的任意一个关键帧观测到的地图点。
- 图14-2标识：红色的“KF地图点”和绿色的“KF1地图点”、“KF2地图点”。这些构成了 ILocalMapPoints。
固定关键帧 (FixedCameras / 不优化固定关键帧):
- 定义：这些关键帧的位姿在本次局部BA中不会被优化，保持固定。它们的存在是为了给待优化的局部关键帧和地图点提供更多的观测约束，增加优化问题的稳定性。
- 组成：通常是二级相连关键帧 (Second-level connected KeyFrames)。这些关键帧能够观测到“局部地图点”，但它们本身不属于“局部关键帧”集合。可以理解为“局部关键帧的邻居的邻居中，那些能看到局部地图点的关键帧”。
- 图14-2标识：灰色的相机图标（“KF1的共视关键帧”，“KF2的共视关键帧”）。这些构成了 IFixedCameras。它们通过观测绿色的地图点（间接也与红色地图点相关）来提供约束。

优化的目标

局部BA的目标是最小化重投影误差 (Reprojection Error)。具体来说，就是调整“局部关键帧”的相机位姿（旋转和平移）和“局部地图点”的三维空间坐标，使得这些地图点在经过各自位姿变换并投影到对应关键帧的图像平面上时，其投影位置与图像上实际观测到的特征点位置之间的差异最小。

图优化视角

局部BA问题通常被构建为一个图优化问题：

顶点 (Vertices): 表示需要优化的变量或固定的状态。
- 关键帧位姿顶点:
  - 待优化: “局部关键帧”的6自由度位姿 (通常用李代数 se(3) 表示，在g2o中对应 g2o::VertexSE3Expmap)。
  - 固定: “固定关键帧”的6自由度位姿 (同样是 g2o::VertexSE3Expmap，但其 setFixed(true) 属性被设为真)。
- 地图点位置顶点:
  - 待优化: “局部地图点”的三维坐标 (通常用三维向量表示，在g2o中对应 g2o::VertexSBAPointXYZ)。
边 (Edges): 表示顶点之间的约束关系，即观测。
- 观测关系: 一条边连接一个地图点顶点和一个观测到该地图点的关键帧位姿顶点。这条边代表了一个约束：“该关键帧以其当前估计位姿观测到了该地图点在其当前估计位置”。
- 边的类型与误差模型:
  - 单目相机 (g2o::EdgeSE3ProjectXYZ): 边表示一个三维地图点投影到单个二维图像平面的过程。误差是二维向量，表示重投影坐标与观测坐标的差。
  - 双目相机/RGB-D相机 (g2o::EdgeStereoSE3ProjectXYZ):
    - 对于双目，边表示一个三维地图点投影到左右两个图像平面的过程，通常误差是三维向量 (u_left, v_left, u_right)。u_right 的目标值通常是根据左图像的u和深度/视差计算得到的。
    - 对于RGB-D，可以将深度信息转换为一个虚拟的右图像坐标，或者直接使用三维点在相机坐标系下的约束。误差通常也是三维向量。

误差定义详解 (以单目 `g2o::EdgeSE3ProjectXYZ` 为例)

书中的 computeError() 函数计算了重投影误差。其核心逻辑是：
获取关键帧位姿顶点（v1，类型为 VertexSE3Expmap）和地图点顶点（v2，类型为 VertexSBAPointXYZ）。
获取图像上的实际观测坐标 obs (一个二维向量 Vector2d，来自 _measurement)。
计算误差 _error 为 obs - cam_project(v1->estimate().map(v2->estimate()))。

详细分解步骤如下：

_vertices[0] (v2): 连接到边的第一个顶点，代表地图点。v2->estimate() 返回该地图点当前估计的世界坐标 $P_w = (X_w, Y_w, Z_w)^T$ 。
_vertices[1] (v1): 连接到边的第二个顶点，代表关键帧。v1->estimate() 返回该关键帧当前估计的位姿 $T_{cw}$ (一个SE3对象，表示从世界坐标系到相机坐标系的变换)。
v1->estimate().map(v2->estimate()): 这一步将世界坐标系下的地图点 $P_w$ 通过关键帧位姿 $T_{cw}$ 变换到该关键帧的相机坐标系下，得到点 $P_c = T_{cw} \cdot P_w = (X_c, Y_c, Z_c)^T$ 。
cam_project(P_c): 这个函数负责将相机坐标系下的三维点 $P_c$ 投影到该关键帧的二维图像平面上。它使用相机的内参矩阵 $K$ (包含焦距 $f_x, f_y$ 和主点 $c_x, c_y$ )：
$ \hat{u} = f_x \frac{X_c}{Z_c} + c_x \hat{v} = f_y \frac{Y_c}{Z_c} + c_y $
如果考虑相机畸变，还需要在投影前后进行畸变校正和反校正。
_measurement (obs): 这是该地图点在该关键帧图像上被实际检测到的特征点的二维坐标 $(u, v)$ 。
_error = obs - cam_project(...): 计算得到的误差是一个二维向量 $(\Delta u, \Delta v) = (u - \hat{u}, v - \hat{v})$ ，即观测值与根据当前估计投影得到的值之间的差异。优化的目标是最小化所有这类误差的平方和（通常加权）。

局部地图优化 (`Optimizer::LocalBundleAdjustment`) 的详细流程

以下是代码中 LocalBundleAdjustment 函数执行的主要步骤：

步骤 1: 将当前关键帧及其共视关键帧加入局部关键帧中 (lLocalKeyFrames)
- 将触发本次局部BA的当前关键帧 pKF 加入 lLocalKeyFrames。
- 获取 pKF 的一级相连关键帧 (即有共视地图点的关键帧，通过 pKF->GetVectorCovisibleKeyFrames())。
- 将这些有效的一级相连关键帧也加入 lLocalKeyFrames。
- 使用 mnBALocalForKF = pKF->mnId 标记这些关键帧，以防止在后续步骤中重复处理或错误添加。
步骤 2: 遍历局部关键帧，将它们观测到的地图点加入局部地图点中 (lLocalMapPoints)
- 遍历 lLocalKeyFrames 中的每一个关键帧 pKFi。
- 获取 pKFi 观测到的所有地图点 vpMPs = pKFi->GetMapPointMatches()。
- 对于每一个有效的地图点 pMP，如果它还没有被加入（通过检查 pMP->mnBALocalForKF != pKF->mnId），则将其加入 lLocalMapPoints，并用 pMP->mnBALocalForKF = pKF->mnId 标记。
步骤 3: 得到能被局部地图点观测到但不属于局部关键帧的关键帧，作为固定关键帧 (lFixedCameras)
- 遍历 lLocalMapPoints 中的每一个地图点 pMP。
- 获取观测到 pMP 的所有关键帧及其在该关键帧中的索引 observations = pMP->GetObservations()。
- 对于每一个观测到 pMP 的关键帧 pKFi:
  - 如果 pKFi 不在 lLocalKeyFrames 中 (通过 pKFi->mnBALocalForKF != pKF->mnId 判断)，并且它还没有被标记为固定关键帧 (通过 pKFi->mnBAFixedForKF != pKF->mnId 判断)。
  - 则将 pKFi 加入 lFixedCameras，并用 pKFi->mnBAFixedForKF = pKF->mnId 标记。这些是二级相连关键帧，提供额外约束。
步骤 4: 构造g2o优化器
- 初始化g2o图优化框架：
  - 创建稀疏优化器 g2o::SparseOptimizer optimizer。
  - 选择线性求解器类型 (如 g2o::LinearSolverEigen 或 g2o::LinearSolverCSparse)。
  - 选择块求解器 (如 g2o::BlockSolver_6_3，表示位姿用6个参数，地图点用3个参数)。
  - 选择优化算法 (如 g2o::OptimizationAlgorithmLevenberg，即Levenberg-Marquardt算法)。
- 设置外部停止标志 pbStopFlag，允许其他线程提前终止优化。
步骤 5: 添加待优化的位姿顶点——局部关键帧的位姿 (lLocalKeyFrames)
- 遍历 lLocalKeyFrames。
- 为每个关键帧 pKFi 创建一个 g2o::VertexSE3Expmap 顶点。
- 设置顶点的初始估计值为该关键帧当前的位姿 pKFi->GetPose()。
- 设置顶点的ID为关键帧的ID pKFi->mnId。
- 特别地：如果关键帧是初始关键帧 (通常 pKFi->mnId == 0)，则将其位姿设置为固定 vSE3->setFixed(true)，不参与优化。
- 将顶点添加到优化器中 optimizer.addVertex(vSE3)。
- 记录最大的关键帧ID maxKFid，用于后续地图点顶点ID的设置，以避免ID冲突。
步骤 6: 添加不优化的位姿顶点——固定关键帧的位姿 (lFixedCameras)
- 遍历 lFixedCameras。
- 为每个关键帧 pKFi 创建一个 g2o::VertexSE3Expmap 顶点。
- 设置顶点的初始估计值为该关键帧当前的位姿。
- 设置顶点的ID。
- 关键：将这些顶点全部设置为固定 vSE3->setFixed(true)，因为它们的位姿不在此次局部BA中优化。
- 将顶点添加到优化器。
- 更新 maxKFid。
步骤 7: 添加待优化的局部地图点作为顶点 (lLocalMapPoints)
- 遍历 lLocalMapPoints。
- 为每个地图点 pMP 创建一个 g2o::VertexSBAPointXYZ 顶点。
- 设置顶点的初始估计值为该地图点当前的世界坐标 pMP->GetWorldPos()。
- 设置顶点的ID。为了避免与关键帧ID冲突，通常设置为 pMP->mnId + maxKFid + 1。
- 将这些地图点顶点设置为边缘化 vPoint->setMarginalized(true)。在基于舒尔补的BA求解中，地图点通常被边缘化，以减小求解系统的规模，提高效率。
- 将顶点添加到优化器。
步骤 8: 每添加完一个地图点之后，对每对关联的地图点和观测到它的关键帧构建边
- 对于每个在步骤7中添加的地图点 pMP：
  - 获取所有观测到该 pMP 的关键帧 observations = pMP->GetObservations()。
  - 对于每一个观测此 pMP 的关键帧 pKFi (可以是局部关键帧或固定关键帧):
    - 如果 pKFi 是有效的 (not bad)。
    - 获取 pMP 在 pKFi 图像上的观测坐标 kpUn.pt.x, kpUn.pt.y (通常是归一化平面坐标或去畸变后的像素坐标)。
    - 创建相应的边对象：
      - 单目: 例如创建一个 g2o::EdgeSE3ProjectXYZ 类型的边。
      - 双目/RGB-D: 例如创建一个 g2o::EdgeStereoSE3ProjectXYZ 类型的边。
    - 设置边的顶点：
      - 将地图点顶点设为边的第一个顶点 (e.g., e->setVertex(0, optimizer.vertex(id_of_map_point_pMP)))。
      - 将观测关键帧顶点设为边的第二个顶点 (e.g., e->setVertex(1, optimizer.vertex(pKFi->mnId)))。
    - 设置边的测量值 e->setMeasurement(obs)，即观测到的图像坐标。
    - 设置边的信息矩阵 (Information Matrix)，即权重的逆协方差矩阵。通常与特征点提取时的图像金字塔层级有关 invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]。信息矩阵一般为 Eigen::Matrix2d::Identity() * invSigma2 (对于单目)。尺度越小（层级越高），不确定性越大，权重越小。
    - 初始阶段：可以设置鲁棒核函数 (e.g., e->setRobustKernel(new g2o::RobustKernelHuber())) 来抑制潜在外点（错误匹配）的影响。Huber阈值 thHuberMono 通常根据卡方分布计算（例如，sqrt(5.991) 对应自由度为2，置信度95%）。
    - 将边添加到优化器 optimizer.addEdge(e)。
    - 同时，将边、关联的关键帧和地图点存储在列表 (vpEdgesMono, vpEdgeKFMono, vpMapPointEdgeMono 等) 中，以便后续进行外点剔除。
步骤 9: 分成两个阶段开始优化
- 检查外部停止标志。
- 第一阶段优化:
  - optimizer.initializeOptimization(): 初始化优化过程。
  - optimizer.optimize(5): 执行少量迭代（例如5次）。
- 再次检查外部停止标志。
- 步骤 9.1: 检测外点，并设置下次不优化/移除鲁棒核 (如果第一阶段优化后继续优化)
  - 遍历在步骤8中创建的所有边 (例如 vpEdgesMono 中的边)。
  - 对于每条边 e 和其关联的地图点 pMP:
    - 如果 pMP 已经无效，则跳过。
    - 计算边的卡方误差 e->chi2()。
    - 检查地图点在相机坐标系下的深度是否为正 e->isDepthPositive() (投影点在相机前方)。
    - 外点判断: 如果 e->chi2() 大于某个阈值 (例如5.991，对应自由度为2，显著性水平0.05的卡方分布临界值)，或者深度为负，则认为这条边对应的观测是外点。
    - 处理外点:
      - e->setLevel(1): 将该边的优化等级设为1，表示在接下来的优化迭代中不考虑这条边（即暂时禁用）。
      - e->setRobustKernel(0): 移除鲁棒核函数。因为第二阶段通常认为是精求解，此时可能已经通过卡方检验识别了大部分外点，或者希望对内点进行更精确的拟合。
- 步骤 9.2: 排除误差较大的边后再次进行优化 (第二阶段优化)
  - optimizer.initializeOptimization(0): 再次初始化优化（参数0可能表示从当前的图结构开始，而不是level 0的边）。
  - optimizer.optimize(10): 执行更多次迭代（例如10次）进行精细求解。
步骤 10: 在优化结束后重新计算误差，剔除边连接误差比较大的关键帧和地图点
- 遍历所有边 (如 vpEdgesMono 和 vpEdgesStereo)。
- 对于每条边 e 及其关联的地图点 pMP 和关键帧 pKFi:
  - 如果 pMP 无效，跳过。
  - 再次检查 e->chi2() 和深度 e->isDepthPositive()。
  - 如果仍然是外点 (误差大于阈值或深度为负)，则将这对 (pKFi, pMP) 添加到待删除列表 vToErase。这意味着这个观测关系被认为是错误的，将在地图中被移除。
- 遍历 vToErase 列表，调用 pKFi->EraseMapPointMatch(pMP) 和 pMP->EraseObservation(pKFi) 来实际断开这些错误的连接。
步骤 11: 更新优化后的关键帧位姿及地图点的位置、平均观测方向等属性
- 更新关键帧位姿: 遍历 lLocalKeyFrames，从优化器中获取优化后的位姿 (e.g., optimizer.vertex(pKFi->mnId)->estimate())，并更新到关键帧对象 pKFi->SetPose(corrected_pose)。
- 更新地图点位置: 遍历 lLocalMapPoints，从优化器中获取优化后的三维坐标 (e.g., optimizer.vertex(id_of_map_point)->estimate())，并更新到地图点对象 pMP->SetWorldPos(corrected_position)。
- 更新地图点其他属性: 地图点的位置更新后，还需要调用 pMP->UpdateNormalAndDepth() 来更新其平均观测方向和在观测帧中的深度等信息，这些信息对于后续的跟踪和建图非常重要。

图 14-2 元素总结

红色相机 (当前KF, KF的共视KF1, KF的共视KF2): 代表 lLocalKeyFrames (待优化的局部关键帧)，其位姿会被优化。
红色地图点 (KF地图点) 和绿色地图点 (KF1地图点, KF2地图点): 代表 lLocalMapPoints (待优化的局部地图点)，其三维位置会被优化。
灰色相机 (KF1的共视关键帧, KF2的共视关键帧): 代表 lFixedCameras (不优化的固定关键帧)，其位姿固定，但它们对绿色地图点的观测为优化提供了额外约束。
所有连线: 代表观测关系，即图优化中的边。线的源头是关键帧，指向被观测的地图点。

结论

局部建图线程中的 LocalBundleAdjustment 是ORB-SLAM2中一个至关重要的优化步骤。通过精心选择一个包含当前关键帧、其紧密相连的关键帧和它们所观测的地图点的局部子图进行优化，并引入固定关键帧作为额外约束，系统能够在保持较高地图精度的同时，显著降低计算量，从而满足实时性要求。两阶段优化和外点剔除策略进一步增强了优化的鲁棒性和准确性。