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。