1. ORB-SLAM2 中优化的变量和误差

  ORB-SLAM2 采用非线性优化的方式进行 BA 优化，由于 BA 的稀疏性（具体表现为雅克比矩阵和 H 矩阵的稀疏性），可以由图优化（将优化表示为图的形式，待优化的变量由顶点表示，误差项由边来表示）显式地形象化表示。在 ORB-SLAM2 中采用 g2o 库来进行图优化求解，需要我们做的就是建立优化问题、确定顶点和边。

1.1 优化的目标函数

  • 三维点到二维特征的映射关系（通过投影矩阵）；
  • 位姿和位姿之间的变换关系（通过三维刚体变换矩阵）；
  • 二维特征到二维特征的匹配关系（通过 F 矩阵）；
  • 其它关系（比如单目中有相似变换关系）。

1.2 需要优化的变量（g2o 的顶点）

  • ① SE(3) 相机位姿 VertexSE3Expmap()
  • ② 地图点坐标 VertexSBAPointXYZ()
  • ③ 闭环检测时的 sim(3) 相机位姿（归一化尺度） VertexSim3Expmap()

1.3 误差来源（g2o 中的边）

  • ① EdgeSE3ProjectXYZ()：BA 中的重投影误差（3D-2D（u,v）误差），将 3D 地图点投影到相机坐标系下的相机平面与观测的像素点产生的重投影误差 ；
  • ② EdgeSE3ProjectXYZOnlyPose()：PoseEstimation 中的重投影误差，将地图点投影到相机坐标系下的相机平面。优化变量只有 pose，地图点位置固定，是一边元（双目中使用的是EdgeStereoSE3ProjectXYZOnlyPoze()）；
  • ③ EdgeSim3()：Sim3 之间的相对误差，优化变量只有 Sim3 表示的 pose，用于 OptimizeEssentialGraph；
  • ④ EdgeSim3ProjectXYZ()：重投影误差。优化变量 Sim3 位姿与地图点，用于闭环检测中的 OptimizeSim3。

1.4 ORB-SLAM2 中的优化函数

1.4.1 局部优化 Optimizer::LocalBundleAdjustment()

  • 在已经处理完队列中的最后一个关键帧之后，并且闭环检测线程没有请求停止局部建图线程，则开始对当前帧进行局部 BA 优化，在 Optimizer::LocalBundleAdjustment() 函数中进行；
  • 当新的关键帧加入到 convisibility graph 时，在关键帧附近进行一次局部优化，如下图所示；
    • Pos3 是新加入的关键帧，其初始估计位姿已经得到，此时，Pos2 是和 Pos3 相连的关键帧，X2 是 Pos3 看到的三维点，X1 是 Pos2 看到的三维点，这些都属于局部信息，共同参与Bundle Adjustment（圈内红色部分）；
    • 同时，Pos1 也可以看到 X1，但它和 Pos3 没有直接的联系，属于 Pos3 关联的局部信息，参与 Bundle Adjustment，但取值保持不变（圈内灰色部分）；
    • Pos0 和 X0 不参与 Bundle Adjustment（圈外灰色部分）。
  • 因此，参与优化的是下图中红色椭圆圈出的部分，其中红色代表取值会被优化，灰色代表取值保持不变。(u,v) 是 X 在 Pos 下的二维投影点，即 X 在 Pos 下的测量（measurement），优化的目标是让投影误差最小。

1.4.2 全局优化 Optimizer::GlobalBundleAdjustemnt

  • 全局优化在单目初始化和闭环矫正时执行，在函数 Optimizer::GlobalBundleAdjustemnt 中；
  • 在全局优化中，所有的关键帧（除了第一帧）和三维点都参与优化，如下图所示：

1.4.3 位姿优化 Optimizer::PoseOptimization()

  • 在 Tracking 线程中进行位姿优化的时候，每进行过一次 PnP 投影操作将地图点投影到当前平面上之后，都会进行一次PoseOptimization 位姿优化最小化重投影误差；
  • 只优化当前帧 pose，地图点固定；
  • 用于 LocalTracking 中运动模型跟踪，参考帧跟踪，地图跟踪 TrackLocalMap，重定位。

1.4.4 闭环处的 Sim3 位姿优化 Optimizer::OptimizeSim3()

  • 在用 RANSAC 求解过 Sim3，以及通过 Sim3 匹配更多的地图点后，对当前关键帧，闭环关键帧，以及匹配的地图点进行优化，获得更准确的Sim3位姿，再去下一步的闭环调整；
  • 当检测到闭环时，闭环连接的两个关键帧的位姿需要通过 Sim3 优化（以使得其尺度一致），在函数 Optimizer::OptimizeSim3() 中实现，优化求解两帧之间的相似变换矩阵，使得二维对应点（feature）的投影误差最小；
  • 如下图所示，Pos6 和 Pos2 为一个可能的闭环，则通过 \((u_{4,2},v_{4,2})\) 和 \((u_{4,6},v_{4,6})\) 之间的投影误差来优化 \(S_{6,2}\)。

1.4.5 Sim3上的位姿优化 Optimizer::OptimizeEssentialGraph()

  • 单目 SLAM 一般都会发生尺度（scale）漂移，因此 Sim3 上的优化是必要的，相对于SE3，Sim3 的自由度要多一个，而且优化的目标是矫正尺度因子，因此优化并没有加入更多的变量（如三维点）；
  • 在检测到闭环时在 sim3 上对所有的位姿进行一次优化，在函数 Optimizer::OptimizeEssentialGraph() 中执行，EssentialGraph 包括所有的关键帧顶点，但是优化边大大减少，包括 spanning tree（生成树），共视权重θ>100 的边，以及闭环连接边。用于闭环检测 Sim3 调整后优化。
  • 定义 sim3 上的残差为： \[ e_{i,j}=log_{Sim3}(S_{ij}S_{jw}S_{iw}^{-1}) \]
    • 其中 \(S_{iw}\) 的初值是尺度为 1 的 pos i 相对于世界坐标系的变换矩阵，\(S_{jw}\) 同理；
    • \(S_{ij}\) 表示 pos i 和 pos j 之间的相对位姿矩阵（sim3 优化之前），表示 \(S_{iw}\) 和 \(S_{jw}\) 之间的测量；
    • 此处相当于认为局部的相对位姿是准确的，而全局位姿有累计误差，是不准确的

R. 参考资料

2019.07.03
wuyanminmax@gmail.com