orb-slam系列 LoopClosing线程 CorrectLoop(十二)

CorrectLoop

/******************************************
 * 	根据回环进行位姿矫正
 * 		1. 请求局部地图线程停止,并且中止现有的全局优化进程
 *		2. 根据当前帧求得的相机位姿(相似变换矩阵)来求解矫正前和矫正后的相邻帧位姿变换矩阵(相似变换矩阵)
 * 		3. 将相邻关键帧的所有地图点都根据更新后的相机位姿(相似变换矩阵)重新计算地图点世界坐标  
 * 		4. 进行地图点融合   将之前匹配的(在ComputeSim3()函数中计算局部地图点和当前帧的匹配)两地图点融合为同一地图点
 * 		5. 根据第3步中计算的地图点重新进行匹配,并融合匹配点和当前关键帧中的地图点
 * 		6. 在地图点融合之后,更新当前关键帧的共视图中各个关键帧的相连关键帧,更新连接之后,将这些相邻关键帧全部加入LoopConnections容器
 * 		7. 根据四种边(1 新检测到的回环边  2 父关键帧与子关键帧的边 3 历史回环关键帧  4 共视图边)对全局地图中的所有关键帧的位姿进行矫正
 * 		8. 根据地图点和关键帧位姿计算重投影误差对全局地图进行优化
 *************************************/
void LoopClosing::CorrectLoop()
{
    cout << "Loop detected!" << endl;

    // Send a stop signal to Local Mapping
    // Avoid new keyframes are inserted while correcting the loop
    mpLocalMapper->RequestStop();

    // If a Global Bundle Adjustment is running, abort it  如果正在运行全局BA优化,那么终止它
    if(isRunningGBA())
    {
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;

        mnFullBAIdx++;

        if(mpThreadGBA)
        {
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
    }

    // Wait until Local Mapping has effectively stopped     等待局部地图线程已经完全停止
    while(!mpLocalMapper->isStopped())
    {
        usleep(1000);
    }

    // Ensure current keyframe is updated   确保当前关键帧不需要进行更新   
    mpCurrentKF->UpdateConnections();

    // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();   //待回环帧的地图点索引出的共视帧
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    // 矫正后的相机位姿    /    未矫正的相机位姿   （该帧   位姿）
    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;   //待回环帧的共视帧  位姿
    CorrectedSim3[mpCurrentKF]=mg2oScw; //矫正sim    待回环点在世界坐标系下正确坐标  sim3 × 回环点世界坐标
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();


    {
        // Get Map Mutex
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
	// 计算矫正后和矫正前的相邻关键帧的相机位姿(相似变换矩阵)      待优化之前的当前帧的共视帧
        for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKFi = *vit;

            cv::Mat Tiw = pKFi->GetPose();

            if(pKFi!=mpCurrentKF)  // 将当前帧的相邻关键帧的相机位姿都根据当前帧的相似变换矩阵进行矫正  sim3矫正
            {
                cv::Mat Tic = Tiw*Twc;    //c是当前帧  i是共视帧
                cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
                cv::Mat tic = Tic.rowRange(0,3).col(3);
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
                g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;    //待回环共视帧 对 世界坐标系的相对位姿 （正确位置 sim3）
                //Pose corrected with the Sim3 of the loop closure
                CorrectedSim3[pKFi]=g2oCorrectedSiw;   //依靠待回环帧的共视帧与 待回环帧的相对位姿 乘 sim3修正  位姿修正成功
            }

            cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
            cv::Mat tiw = Tiw.rowRange(0,3).col(3);
            g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
            //Pose without correction
            NonCorrectedSim3[pKFi]=g2oSiw;  //没有进行sim3相乘
        }

        // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
        // 将相邻关键帧的所有地图点     都根据更新后的相机位姿(相似变换矩阵)重新计算地图点世界坐标  
        //待回环帧
        for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
        {
            KeyFrame* pKFi = mit->first;
            g2o::Sim3 g2oCorrectedSiw = mit->second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();

            g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];   //无 sim3  待回环共视帧 在世界坐标系位置（漂移的世界坐标系）

            vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
            for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)   //进行了 地图点 对于世界坐标系的修正
            {
                MapPoint* pMPi = vpMPsi[iMP];
                if(!pMPi)
                    continue;
                if(pMPi->isBad())
                    continue;
                if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId)
                    continue;

                // Project with non-corrected pose and project back with corrected pose
                cv::Mat P3Dw = pMPi->GetWorldPos();  //未sim3  待回环共视帧kf1 中的 地图点i 在世界坐标系位置（漂移的世界坐标系）
                Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
                Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));  // 依靠待回环帧共视帧地图点 与 待回环帧共视帧的关系 恢复待回环帧共视帧地图点

                cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                pMPi->SetWorldPos(cvCorrectedP3Dw);
                pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                pMPi->mnCorrectedReference = pKFi->mnId;
                pMPi->UpdateNormalAndDepth();
            }

            // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
            Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
            Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
            double s = g2oCorrectedSiw.scale();

            eigt *=(1./s); //[R t/s;0 1]

            cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);

            pKFi->SetPose(correctedTiw);   //待回环点共视帧 对于位姿的修正

            // Make sure connections are updated
            pKFi->UpdateConnections();
        }

        // Start Loop Fusion
        // Update matched map points and replace if duplicated
        // 进行地图点融合   将匹配的两地图点融合为同一地图点    对当前帧 的 地图点修正后融合 添加到地图
        for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
        {
            if(mvpCurrentMatchedPoints[i])
            {
                MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
                MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
                if(pCurMP)
                    pCurMP->Replace(pLoopMP);
                else
                {
                    mpCurrentKF->AddMapPoint(pLoopMP,i);
                    pLoopMP->AddObservation(mpCurrentKF,i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        }

    }

    // Project MapPoints observed in the neighborhood of the loop keyframe
    // into the current keyframe and neighbors using corrected poses.
    // Fuse duplications.
    SearchAndFuse(CorrectedSim3);

    /////////////////////////////////////////////////////////////////////////////////////////////////////

    // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
    map<KeyFrame*, set<KeyFrame*> > LoopConnections;   
    // 在地图点融合之后,更新当前关键帧的共视图中各个关键帧的相连关键帧,更新连接之后,将这些相邻关键帧全部加入LoopConnections容器
    for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();    //返回共视连接关键帧  未进行重新的共视地图点关系计算

        // Update connections. Detect new links.   更新链接,将当前帧的关联关键帧的关联关键帧加入LoopConnections容器,不包括直接相邻的和当前帧的关联关键帧
	// 实际就是一系列回环的集合,保证回环的连续性
        pKFi->UpdateConnections();
        LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();    //重新进行地图点关系计算后  共视连接的关键帧   权重大于15的共视关键帧
        for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
        {
            LoopConnections[pKFi].erase(*vit_prev);   //去除 老关联关键帧的  共视关键帧
        }
        for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
        {
            LoopConnections[pKFi].erase(*vit2);    //去除老关联关键帧
        }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////

    // Optimize graph    进行位姿图优化
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

    mpMap->InformNewBigChange();

    // Add loop edge
    mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);

    // Launch a new thread to perform Global Bundle Adjustment  开辟线程进行全局BA进行图优化
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);

    // Loop closed. Release Local Mapping.
    mpLocalMapper->Release();    

    mLastLoopKFid = mpCurrentKF->mnId;   
}

// 根据矫正后的相机相似矩阵位姿重新匹配回环点和当前关键帧,并融合得到的关键帧中匹配点和回环地图点
void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
{
    ORBmatcher matcher(0.8);   
    //矫正后的  待回环帧的共视帧
    for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
    {
        KeyFrame* pKF = mit->first;

        g2o::Sim3 g2oScw = mit->second;
        cv::Mat cvScw = Converter::toCvMat(g2oScw);

        vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));  //融合更改后的地图点（ 
        matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);    //未矫正的地图点 矫正的位置cvScw  对于每个共视帧 和 全部的共视帧地图点进行fuse 找到可以替代的存入 vpReplacePoints[imp]

        // Get Map Mutex
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
        const int nLP = mvpLoopMapPoints.size();
        for(int i=0; i<nLP;i++)
        {
            MapPoint* pRep = vpReplacePoints[i];
            if(pRep)
            {
                pRep->Replace(mvpLoopMapPoints[i]);   //替换
            }
        }
    }
}

1）mvpCurrentConnectedKFs 待回环帧及其共视帧

    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();   //待回环帧的地图点索引出的共视帧
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    // 矫正后的相机位姿    /    未矫正的相机位姿   （该帧   位姿）
    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;   //待回环帧的共视帧  位姿
    CorrectedSim3[mpCurrentKF]=mg2oScw; //矫正sim    待回环点在世界坐标系下正确坐标  sim3 × 回环点世界坐标
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();

Twc是未矫正 的 逆位姿
2）共视Tiw是未矫正位姿，Tic = TiwTwc;得到 共视帧到当前帧的位姿变化
Sim3 g2oCorrectedSiw = g2oSicmg2oScw; 是共视帧修正后的 真正位姿
NonCorrectedSim3[pKFi]=g2oSiw; //没有进行sim3相乘
3）待回环帧共视帧所有的地图点
P3Dw = pMPi->GetWorldPos(); 未修正的地图点位置
eigP3Dw = Converter::toVector3d(P3Dw);未修正位置
eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw)); 修正位置
修正了待回环帧共视帧所有的地图点
4）完成对待回环帧共视帧的位姿修正
5）然后进行地图点融合，以整体pLoopMP的地图点覆盖当前帧地图点。
地图点融合 地图点 按照 修正位姿投射 在共视帧的匹配点
6）待回环帧共视帧 的二级共视帧 和 待回环的共视帧删除
剩下的LoopConnections 表示共视帧在建立回环之后，新匹配到的共视关系帧
进行位姿优化

OptimizeEssentialGraph

/****************************************
 * 	优化必要图
 * 		pMap(in):由关键帧和地图点组成的整个地图
 * 		pLoopKF(in):检测到的回环关键帧
 * 		pCurKF(in): 当前关键帧
 * 		NonCorrectedSim3: 未调整位姿的sim相似变换矩阵
 * 		CorrectedSim3: 矫正之后的sim相似变换矩阵
 * 		LoopConnections: 更新链接,将当前帧的关联关键帧的关联关键帧加入LoopConnections容器,不包括直接相邻的和当前帧的关联关键帧   实际就是一系列回环的集合,保证回环的连续性
 * 		bFixScale: 是否修正尺度
 * 	步骤:
 * 		1. 设置优化器
 * 		2. 设置关键帧位姿节点
 * 		3. 设置回环边(新检测到的回环边)   (位姿用矫正的相似变换矩阵)
 * 		4. 设置三种普通边  1>父节点边  2> 回环边(此回环边与3中的回环边不同在于:包括历史检测到的回环边)  3> 共视边   (位姿用矫正的相似变换矩阵)
 * 		5. 进行优化
 * 		6. 恢复优化后的节点位姿(相似变换矩阵)
 * 		7. 根据关键帧的位姿调整地图点的坐标
 *******************************************/
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
                                       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
                                       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
                                       const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)
{
    // Setup optimizer
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(false);
    g2o::BlockSolver_7_3::LinearSolverType * linearSolver =
           new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();
    g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    solver->setUserLambdaInit(1e-16);
    optimizer.setAlgorithm(solver);
    // 整个地图的关键帧
    const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
    //整个地图的地图点
    const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();

    // 最大关键帧的id
    const unsigned int nMaxKFid = pMap->GetMaxKFid();

    // 地图中所有关键帧的未矫正的sim变换矩阵   
    vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vScw(nMaxKFid+1);
    // 地图中所有关键帧的矫正之后的sim变换矩阵
    vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vCorrectedSwc(nMaxKFid+1);
    vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);

    const int minFeat = 100;

    // Set KeyFrame vertices   设置关键帧位姿(相似变换矩阵)结点
    for(size_t i=0, iend=vpKFs.size(); i<iend;i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();

        const int nIDi = pKF->mnId;

        LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);    //待回环帧 共视帧 要调整

        if(it!=CorrectedSim3.end())   // 如果已经计算过矫正sim矩阵的关键帧直接添加相似变换矩阵节点
        {
            vScw[nIDi] = it->second;
            VSim3->setEstimate(it->second);
        }
        else    // 否则将关键帧的位姿变换矩阵转换为相似变换矩阵加入到图优化中
        {   
            //图优化  没矫正的 应该矫正的帧
            Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());
            Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());
            g2o::Sim3 Siw(Rcw,tcw,1.0);
            vScw[nIDi] = Siw;
            VSim3->setEstimate(Siw);
        }

        if(pKF==pLoopKF) // 如果是回环的两端的关键帧,则不进行优化
            VSim3->setFixed(true);

        VSim3->setId(nIDi);
        VSim3->setMarginalized(false);
        VSim3->_fix_scale = bFixScale;

        optimizer.addVertex(VSim3);

        vpVertices[nIDi]=VSim3;
    }


    set<pair<long unsigned int,long unsigned int> > sInsertedEdges;

    const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();

    // Set Loop edges   设置回环边  边的两端分别是回环的两个端点,回环的初始端点是一个关键帧集合,末端点与该关键帧集合形成若干的边   该帧first 相连关键帧second
    for(map<KeyFrame *, set<KeyFrame *> >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        const long unsigned int nIDi = pKF->mnId;
        const set<KeyFrame*> &spConnections = mit->second;
        const g2o::Sim3 Siw = vScw[nIDi];    //未矫正
        const g2o::Sim3 Swi = Siw.inverse();

        for(set<KeyFrame*>::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++)  // i是未矫正待回环帧  j是待回环帧相邻帧
        {
            const long unsigned int nIDj = (*sit)->mnId;
            if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit)<minFeat)
                continue;

            const g2o::Sim3 Sjw = vScw[nIDj];
            const g2o::Sim3 Sji = Sjw * Swi;

            g2o::EdgeSim3* e = new g2o::EdgeSim3();
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
            e->setMeasurement(Sji);

            e->information() = matLambda;

            optimizer.addEdge(e);

            sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));
        }
    }

    // Set normal edges   设置正常的边
    for(size_t i=0, iend=vpKFs.size(); i<iend; i++)
    {
        KeyFrame* pKF = vpKFs[i];
	
        const int nIDi = pKF->mnId;

        g2o::Sim3 Swi;

        LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);

        if(iti!=NonCorrectedSim3.end())  
            Swi = (iti->second).inverse();
        else
            Swi = vScw[nIDi].inverse();

        KeyFrame* pParentKF = pKF->GetParent();

        // Spanning tree edge  插入三种边
	// 第一种边: 边两端为父关键帧位姿(未矫正的相似变换矩阵)和当前关键帧位姿(未矫正的相似变换矩阵)
        if(pParentKF)
        {
            int nIDj = pParentKF->mnId;

            g2o::Sim3 Sjw;

            LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);

            if(itj!=NonCorrectedSim3.end())
                Sjw = itj->second;
            else
                Sjw = vScw[nIDj];
	    // 两关键帧之间的相似变换矩阵
            g2o::Sim3 Sji = Sjw * Swi;

            g2o::EdgeSim3* e = new g2o::EdgeSim3();
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
            e->setMeasurement(Sji);

            e->information() = matLambda;
            optimizer.addEdge(e);
        }

        // Loop edges
        // 第二种边: 回环边,边的两边分别是当前关键帧和其回环关键帧的位姿(未矫正的相似变换矩阵)
        const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();
        for(set<KeyFrame*>::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++)
        {
            KeyFrame* pLKF = *sit;
            if(pLKF->mnId<pKF->mnId)
            {
                g2o::Sim3 Slw;

                LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);

                if(itl!=NonCorrectedSim3.end())
                    Slw = itl->second;
                else
                    Slw = vScw[pLKF->mnId];

                g2o::Sim3 Sli = Slw * Swi;
                g2o::EdgeSim3* el = new g2o::EdgeSim3();
                el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pLKF->mnId)));
                el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
                el->setMeasurement(Sli);
                el->information() = matLambda;
                optimizer.addEdge(el);
            }
        }

        // Covisibility graph edges
        // 第三种边:共视图边  边的两端分别是当前关键帧位姿和共视关键帧的位姿(未矫正的相似变换矩阵)
        const vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);
        for(vector<KeyFrame*>::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++)
        {
            KeyFrame* pKFn = *vit;
            if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn))
            {
                if(!pKFn->isBad() && pKFn->mnId<pKF->mnId)
                {
                    if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId))))
                        continue;

                    g2o::Sim3 Snw;

                    LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);

                    if(itn!=NonCorrectedSim3.end())
                        Snw = itn->second;
                    else
                        Snw = vScw[pKFn->mnId];

                    g2o::Sim3 Sni = Snw * Swi;

                    g2o::EdgeSim3* en = new g2o::EdgeSim3();
                    en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFn->mnId)));
                    en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
                    en->setMeasurement(Sni);
                    en->information() = matLambda;
                    optimizer.addEdge(en);
                }
            }
        }
    }

    // Optimize!  进行优化!!!
    optimizer.initializeOptimization();
    optimizer.optimize(20);

    unique_lock<mutex> lock(pMap->mMutexMapUpdate);

    // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
    // 位姿恢复  从相似变换矩阵变换到欧式变换矩阵
    for(size_t i=0;i<vpKFs.size();i++)
    {
        KeyFrame* pKFi = vpKFs[i];

        const int nIDi = pKFi->mnId;

        g2o::VertexSim3Expmap* VSim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(nIDi));
        g2o::Sim3 CorrectedSiw =  VSim3->estimate();
        vCorrectedSwc[nIDi]=CorrectedSiw.inverse();
        Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();
        Eigen::Vector3d eigt = CorrectedSiw.translation();
        double s = CorrectedSiw.scale();

        eigt *=(1./s); //[R t/s;0 1]

        cv::Mat Tiw = Converter::toCvSE3(eigR,eigt);

        pKFi->SetPose(Tiw);
    }

    // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
    // 修正地图点坐标   用优化后的关键帧位姿重新计算地图点位姿.
    for(size_t i=0, iend=vpMPs.size(); i<iend; i++)
    {
        MapPoint* pMP = vpMPs[i];

        if(pMP->isBad())
            continue;

        int nIDr;
        if(pMP->mnCorrectedByKF==pCurKF->mnId)
        {
            nIDr = pMP->mnCorrectedReference;
        }
        else
        {
            KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
            nIDr = pRefKF->mnId;
        }


        g2o::Sim3 Srw = vScw[nIDr];
        g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];

        cv::Mat P3Dw = pMP->GetWorldPos();
        Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
        Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));

        cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
        pMP->SetWorldPos(cvCorrectedP3Dw);   //更新了地图点坐标

        pMP->UpdateNormalAndDepth();
    }
}

1）所有帧，假设已经矫正 直接添加相似变换矩阵节点 未矫正 添加位姿 回环两端 不进行优化
2）矫正LoopConnections 新建立的共视关系
回环共视帧新建立的共视关系帧世界坐标系 到 回环共视帧世界坐标系倒数
优化函数 Sji × Scmiw *Scmwj（没矫正）

3)优化正常边 父帧sjw 正常siw
回环 正常siw 回环变slw
共视边权重大于100 snw 正常siw
4）优化后更正位姿
5）进行全局优化