ORB-SLAM优化

Posted 2020-09-02 SLAM-BJTShang

ORB-SLAM中优化使用g2o库，分为：

1. BundleAdjustment

• GlobalBundleAdjustment：用于单目初始化的CreateInitialMapMonocular函数以及闭环优化的RunGlobalBundleAdjustment函数（在闭环结束前新开一个线程，全局优化，在此之前会OptimizeEssentialGraph，论文中说其实这里全局优化提升的精度有限）。
• LocalBundleAdjustment：用于LocalMapping线程中剔除关键帧之前的局部地图优化。

2. PoseOptimization

• 只优化pose，地图点固定
• 用于LocalTracking中运动模型跟踪，参考帧跟踪，地图跟踪TrackLocalMap，重定位。

3. OptimizeEssentialGraph

• EssentialGraph包括所有的关键帧顶点，但是优化边大大减少，包括spanning tree（生成树），共视权重θ>100的边，以及闭环连接边。
• 用于闭环检测Sim3调整后优化。

4. OptimizeSim3

• 在用RANSAC求解过Sim3，以及通过Sim3匹配更多的地图点后，对当前关键帧，闭环关键帧，以及匹配的地图点进行优化，获得更准确的Sim3位姿，再去下一步的闭环调整。

使用到的g2o顶点包括：

1. VertexSE3Expmap()：SE(3)位姿

2. VertexSim3Expmap()：Sim(3)位姿

3. VertexSBAPointXYZ()：地图点坐标

使用到的g2o边包括：

1. EdgeSE3ProjectXYZ()：BA中的重投影误差（3D-2D（u,v）误差），将地图点投影到相机坐标系下的相机平面。

2. EdgeSE3ProjectXYZOnlyPose()：PoseEstimation中的重投影误差，将地图点投影到相机坐标系下的相机平面。优化变量只有pose，地图点位置固定，是一边元，双目中使用的是EdgeStereoSE3ProjectXYZOnlyPoze()。

3. EdgeSim3()：Sim3之间的相对误差。优化变量只有Sim3表示的pose，用于OptimizeEssentialGraph。

4. EdgeSim3ProjectXYZ()：重投影误差。优化变量Sim3位姿与地图点，用于闭环检测中的OptimizeSim3。