ORB-SLAM2中的g2o

一. 图优化库g2o

图优化，是把优化问题表现成图（Graph）的一种形式。这里的图是图论意义上的图。一个图由若干个顶点（Vertex），以及连接着这些定点的边（Edge）组成。进而，用顶点表示优化变量，用边表示误差项。于是，对于任意一个如下形式的非线性最小二乘问题，我们可以构建与之对应的一个图：

$\underset{a,b,c}{\min }\frac{1}{2}{\sum }_{i=1}^N{\Vert y_i-exp(a{x}_i^2+b{x}_i+c)\Vert }^2$

g2o是图优化一个求解库，通常做图优化的时候需要实现以下步骤：

1.选择你想要的图里的节点与边的类型，确定它们的参数化形式；
2.往图里加入实际的节点和边；
3.选择初值，开始迭代；
4.每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
5求解稀疏线性方程HkΔx=−bk，得到梯度方向；
6.继续用GN或LM进行迭代。如果迭代结束，返回优化值。

实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。

具体的实现细节可以参考高博的博客：https://www.cnblogs.com/gaoxiang12/p/5304272.html

2.ORB-SLAM2中的g2o

1.全局BA

//pMap中所有的MapPoints和关键帧做bundle adjustment优化
//这个全局BA优化在ORB-SLAM2中有俩个地方有用
//a.单目初始化：CreateInitialMapMonocular 函数
//b.闭环优化：RunGlobalBundleAdjustment函数
/** 
 * @brief bundle ajustment Optimization
 * 3D-2D 最小化重投影误差：e = (u, v) - project(Tcw*Pw)
 * 1. Vertex : g2o::VertexSE3Expmap(), 即当前的Tcw
 *             g2o::VertexSBAPointXYZ(),MapPoint的mWorldPos
 * 2.Edge: 
 *    - g2o::EdgeSE3ProjectXYZ(), BaseBinaryEdge
 *         + Vertex: 待优化当前帧的Tcw
 *         + Vertex: 待优化MapPoint的mWorldPos
 *         + measurement：MapPoint在当前帧中的二维位置(u ,v )
 *         + InfoMatrix: invSigma2 (与特征点所在的尺度有关)
 * @param  vpKFs 关键帧
 *         vpMP  MapPoints
 *         nIterations 迭代次数（20次）
 *         pbStopFlag 是否强制暂停
 *         nLoopKF 关键帧的个数
 *         bRobust 是否使用该函数
 */
 void Optimizer::BundleAdjustment(const vector &vpKFs, const vector &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

2.单帧位姿优化

// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位
/**
 * @brief Pose Only Optimization
 * 3D-2D 最小二乘投影误差 e = (u, v) - project(Tcw*Pw)
 * 只优化Frame的Tcw, 不优化MapPoints的坐标
 * 1. Vertex： g2o::VertexSE3Expmap()，即当前帧的Tcw
 * 2. Edge：
 *       - g2o::EdgeSE3ProjectXYZOnlyPose(), BaseUnaryEdge
 *             + Vertex: 待优化当前帧的Tcw
 *             + measurement: MapPoint 在当前帧的二维位置（u，v）
 *             + InfoMatrix: invSigma2 (与特征点所在的尺度有关)
 *       - g2o::EdgeStereoSE3ProjectXYZOnlyPose(), BaseUnaryEdge
 *             + Vertex: 待优化当前帧的Tcw
 *             + measurement: MapPoint在当前帧的二维位置（ul，v，ur）
 *             + InfoMatrix: invSigma2 (与特征点所在的尺度有关)
 * @param pFrame Frame
 * @return inliers 数量
 * /
 int Optimizer::PoseOptimization(Frame *pFrame)

3.局部地图 (Local Graph) BA优化

/** 
* @brief  Local Bundle Adjustment
* 1. Vertex:
*     -g2o::VertexSE3Expmap(), LocalKeyFrames, 即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
*     -g2o::VertexSE3Expmap(), FixedCameras, 即能观测到LocalMapPoints的关键帧（并且不属于LocalKeyFrame）的位姿，在优化中这些关键帧的位姿不变
*     -g2o::VertexSBAPointXYZ(), LocalMapPoints, 即LocalKeyFrames能观测到的所有MapPoints的位姿
* 2. Edge：
*     -g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
*      +Vertex：关键帧的Tcw, MapPoint的Pw
*      +measurement: MapPoint在关键帧中的二维位置（u,v）
*      +InfoMatrix: invSigma2 (与特征点所在的尺度有关)
*    -g2o::EdgeStereoProjectXYZ()，BaseBinaryEdge
*      +Vertex: 关键帧的Tcw, MapPoint的Pw
*      +measurement: MapPoint在关键帧中的二维位置（ul, v, ur）
*      +InfoMatrix: invSigma2 (与特征点所在的尺度有关)
*
*   @param pKF  KeyFrame
*   @param pbStopFlag 是否停止优化的标志
*   @param pMap 在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate
* /
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)

4.本质图（Essential Graph）位姿优化

/** 
* @brief 闭环检测后，Essential Graph优化
* 1. Vertex：
*      -g2o::VertexSE3ExpMap，Essential graph中关键帧的位姿
* 2. Edge：
*     -g2o::EdgeSim3(), BaseBinaryEdge
*           + Vertex: 关键帧的Tcw
*           + measurement: 经过CorrectLoop函数步骤2，Sim3传播校正后的关键帧相对位姿 ，若没有校正则用原来的位姿
*           + InfoMatrix: 单位矩阵
* @param pMap   全局地图
* @param pLoopKF  闭环匹配上的关键帧
* @param pCurKF 当前关键帧
* @param NonCorrectedSim3 未经过Sim3传播调整过的关键帧位姿
* @param CorrectedSim3 经过Sim3传播调整过的关键帧位姿
* @param LoopConnections 因闭环时MapPoints调整而新生成的边
*/
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
                                       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
                                       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
                                       const map > &LoopConnections, const bool &bFixScale)

5.俩帧间Sim3优化

/** 
* @brief 形成闭环时进行Sim3优化
* 1. Vertex: 
*         - g2o::VertexSE3Expmap()，俩个关键帧的位姿
*         - g2o::VertexSBAPointXYZ()，俩个关键帧共有的MapPoints
* 2. Edge:
*         -g2o::EdgeSim3ProjectXYZ(), BaseBinaryEdge
*                 +Vertex：关键帧的Sim3, MapPoint的Pw
*                 +measurement: MapPoint在关键帧的二维位置（u,v）
*                 +InfoMatrix：InvSigma2 (与特征点所在的尺度有关)
*         -g2o::EdgeInverseSim3ProjectXYZ(), BaseBinaryEdge
*                 +Vertex: 关键帧的Sim3, MapPoint的Pw
*                 +measurement: MapPoint在关键帧中的二维位置（u, v）
*                 +InfoMatrix: invSigma2(与特征点所在的尺度有关)
* 
*  @param pKF1  KeyFrame
*  @param pKF2  KeyFrame
*  @param vpMatches1 俩个关键帧的匹配关系
* @param g2oS12 俩个关键帧间的Sim3变换
* @param th2 核函数阈值
* @param bFixScale 是否优化尺度，单目进行尺度优化，双目不进行尺度优化
*/
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)

参考： https://blog.csdn.net/potxxx/article/details/87889821