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。