ORB-SLAM（十二）優化

ORB-SLAM中優化使用g2o庫，先復習一下g2o的用法，上類圖

其中SparseOptimizer就是我們需要維護的優化求解器，他是一個優化圖，也是一個超圖（包含若干頂點和一元二元多元邊），怎樣定義圖的頂點（優化變量_estimate）和邊（誤差項_error）是用戶需要考慮的問題，可以從g2o/types查找是否已經有定義好的頂點或邊，若沒有，需要自己去實現。

自己實現的時候注意，基本都是去繼承BaseVertex<D,T>; BaseUnaryEdge<D,E,VertexXi>...這幾個模板類，根據需要實現(override)虛函數．

• virtual void setToOriginImpl() 設定優化變量0點 _estimate = 0 ;
• virtual void oplusImpl(const double* v)實現優化變量的增量計算，特別是優化變量不在歐式空間中，沒有的"＋"定義時。例如李代數中，需要使用左乘或者右乘定義而不是直接的加法;
• void computeError() 計算估計值和測量值之間的誤差項_error
• virtual void linearizeOplus() 計算雅可比的解析形式_jacobianOplus[i]，每一個雅可比的類型為MatrixX::MapType， 參考Eigen::Map類 https://eigen.tuxfamily.org/dox/classEigen_1_1Map.html，可以認為是將一塊內存數據構造成一個矩陣，比較靈活地適應相對各個優化變量的雅可比形式。

這張類圖的下半部分就是我們初始化優化求解器時需要指定的求解方法。

求解的梯度下降算法可以選擇GN，LM（最常用），或者DogLeg；

算法求解器還包括兩部分，

• 計算目標函數雅可比和海塞（或者近似的海塞），以及執行Schur complement的BlockSolver，需要指定優化變量的維度，常見的有

  •   //variable size solver
      using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
    
      // solver for BA/3D SLAM
      using BlockSolver_6_3 = BlockSolverPL<6, 3>;
    
      // solver fo BA with scale
      using BlockSolver_7_3 = BlockSolverPL<7, 3>;
    
      // 2Dof landmarks 3Dof poses
      using BlockSolver_3_2 = BlockSolverPL<3, 2>;

    可以設置為動態的BlockSolverX

• 求解線性方程組 Hx = b (linear problem constructed from _jacobianOplus and _error)的線性求解器，完全采用第三方的線性代數庫，主要采用Cholesky分解和PCG迭代，具體可以選擇的有
  • Cholmod，CSparse （以上兩者為比較著名的線性代數庫），
  • PCG （pre-conditioner is block Jacobi），
  • Dense（dense Cholesky decomposition），
  • 或者ORB-SLAM中使用的Eigen（sparse Cholesky decoposition from Eigen）。

因此，ORB-SLAM中優化求解器初始化過程如下：

g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
// 線性方程求解器
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
// 稀疏矩陣塊求解器
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// 梯度下降算法
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

 

ORB中使用的這些優化函數是非常重要的，在視覺SLAM中有很強的通用性，自己實現的時候完全可以參考其實現方法。分為：

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。