6、ORB-SLAM闭环检测之通过g2o优化求解出来的sim3

1 OptimizeSim3（）

经过前面那么多步骤，终于来到闭环检测的最后一步了，只要sim3优化通过则证明真正检测到了闭环，然后接下来的工作才是闭环矫正。
关于g2o优化的知识，如果没有学习的话，推荐看我之前的关于g2o介绍以及编程步骤的博客。
首先把整个优化代码贴在下面，然后一步步图解。这里面还包含了g2o顶点和边的编程套路，关于此可以阅读g2o顶点编程和g2o边编程。

int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{
   
    // Step 1：初始化g2o优化器
    // 先构造求解器
    g2o::SparseOptimizer optimizer;
    // 构造线性方程求解器，Hx = -b的求解器
    g2o::BlockSolverX::LinearSolverType * linearSolver;
    // 使用dense的求解器，（常见非dense求解器有cholmod线性求解器和shur补线性求解器）
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
    // 使用L-M迭代
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // 内参矩阵
    const cv::Mat &K1 = pKF1->mK; //取出当前关键帧内参
    const cv::Mat &K2 = pKF2->mK; //取保留闭环候选关键帧内参

    // Camera poses
    const cv::Mat R1w = pKF1->GetRotation();    //取出当前关键帧的旋转矩阵R
    const cv::Mat t1w = pKF1->GetTranslation(); //取出当前关键帧的平移向量t
    const cv::Mat R2w = pKF2->GetRotation();    //取出保留闭环候选关键帧的旋转矩阵R
    const cv::Mat t2w = pKF2->GetTranslation(); //取出保留闭环候选关键帧的平移向量t

    // Step 2: 设置Sim3 作为顶点         //要优化两帧之间得sim3，待优化变量sim3作为顶点
    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap(); //new一个sim3作为顶点   
    // 根据传感器类型决定是否固定尺度
    vSim3->_fix_scale=bFixScale;//单目固定 ，双目RGBD不固定
    vSim3->setEstimate(g2oS12);//保留闭环候选关键帧到当前关键帧的粗Sim3变换放到优化变量顶点中，作为初始估计值
    vSim3->setId(0);//设置为第0个顶点
    // Sim3 需要优化
    vSim3->setFixed(false);                             // 因为要优化Sim3顶点，不固定它，所以设置为false
    vSim3->_principle_point1[0] = K1.at<float>(0,2);    // 光心横坐标cx   //设置各种顶点参数
    vSim3->_principle_point1[1] = K1.at<float>(1,2);    // 光心纵坐标cy
    vSim3->_focal_length1[0] = K1.at<float>(0,0);       // 焦距 fx
    vSim3->_focal_length1[1] = K1.at<float>(1,1);       // 焦距 fy
    vSim3->_principle_point2[0] = K2.at<float>(0,2);
    vSim3->_principle_point2[1] = K2.at<float>(1,2);
    vSim3->_focal_length2[0] = K2.at<float>(0,0);
    vSim3->_focal_length2[1] = K2.at<float>(1,1);
    optimizer.addVertex(vSim3);//sim3顶点加入到优化器里

    // Set MapPoint vertices
    // Step 3: 设置地图点作为顶点
    const int N = vpMatches1.size();//取出当前关键帧与保留闭环候选关键帧的匹配地图点（内点）大小
    // 获取pKF1的地图点
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();//当前关键帧的地图点取出来

    vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;         //pKF2对应的地图点到pKF1的投影边   //保留闭环候选关键帧对应的地图点到当前关键帧的投影边 
    vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;  //pKF1对应的地图点到pKF2的投影边   //当前关键帧对应的地图点到保留闭环候选关键帧的投影边
    vector<size_t> vnIndexEdge;                         //边的索引

    vnIndexEdge.reserve(2*N); //边索引的vector，预留空间
    vpEdges12.reserve(2*N);   //存储正向边的vector，预留空间
    vpEdges21.reserve(2*N);   //存储反向边的vector，预留空间

    // 核函数的阈值
    const float deltaHuber = sqrt(th2);  //设置鲁棒核函数阈值10

    int nCorrespondences = 0;//匹配对，初始为0

    // 遍历每对匹配点
    for(int i=0; i<N; i++)//循环当前关键帧与保留闭环候选关键帧的匹配地图点索引
    {
   
        if(!vpMatches1[i])//无匹配，就跳出
            continue;

        // pMP1和pMP2是匹配的地图点
        MapPoint* pMP1 = vpMapPoints1[i];//取出当前关键帧的地图点
        MapPoint* pMP2 = vpMatches1[i];  //取出保留闭环候选关键帧中对应的匹配地图点，vpMatches1[i]，i表示kF1第i个地图点索引，vpMatches1[i]值存放的是kF2中i对应的地图点

        // 保证顶点的id能够错开
        const int id1 = 2*i+1;   //地图点也是待优化的变量，每个i对应2个地图点顶点，保证顶点id错开
        const int id2 = 2*(i+1);

        // i2 是 pMP2 在pKF2中对应的索引
        const int i2 = pMP2->GetIndexInKeyFrame(pKF2);//取出保留闭环候选关键帧对应的地图点的特征点索引

        if(pMP1 && pMP2)//如果当前关键帧的地图点和保留候选关键帧对应的地图点都存在
        {
   
            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)//并且不是Bad，且保留闭环候选关键帧对应的地图点的特征点索引也存在
            {
   
                // 如果这对匹配点都靠谱，并且对应的2D特征点也都存在的话，添加PointXYZ顶点
                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();//定义地图点顶点，new了一个地图点顶点
                // 地图点转换到各自相机坐标系下的三维点
                cv::Mat P3D1w = pMP1->GetWorldPos();//取出当前关键帧的地图点的世界坐标系
                cv::Mat P3D1c = R1w*P3D1w + t1w;//转换到当前关键帧的相机坐标系下
                vPoint1->setEstimate(Converter::toVector3d(P3D1c));//把当前关键帧的相机坐标系作为地图顶点的初始值（转换成EigenEigen::Vector3d类型）
                vPoint1->setId