7.1 闭环检测之闭环矫正（位姿传播与地图点修正）

前面介绍了 闭环关键帧是如何确定的，包括 闭环候选关键帧的层层选拨、通过词袋匹配估计sim3变换、根据估计的sim3寻找更多的特征点匹配、用g2o进行sim3优化。 得到优化后的sim3变换之后，还有一步：通过sim3将闭环帧的观测点投影到当前帧继续寻找更多匹配， int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th) 。这块和前面大同小异，很容易看懂，就不写了，下面开始来看看ORBSLAM是如何进行闭环矫正的吧。

1 闭环矫正CorrectLoop()

1.1 闭环矫正之前的准备

闭环检测是为了消除SLAM系统的长期累积误差，也就是说这本质是一个优化问题，而且还是从第一帧关键帧开始到闭环关键帧之间的优化，所以进行闭环矫正之前应该把局部建图线程以及全局优化线程停掉。因为局部建图线程会插入新的关键帧，会对闭环检测的全局优化造成影响。这块没怎么想明白，这意思是闭环矫正的时候只进行跟踪，不保存关键帧信息了。除非闭环矫正很快，不让很容易跟丢吧。以后回来填坑吧。

	cout << "Loop detected!" << endl;
	mpLocalMapper->RequestStop();//在闭环矫正时，需要停止局部建图线程

    if(isRunningGBA())
    {
   
        // 如果有全局BA在运行，终止掉，迎接新的全局BA
        unique_lock<mutex> lock(mMutexGBA);//锁住全局BA，互斥锁的概念，在这里拿到锁，代表全局BA线程只能由当前线程调用。
        mbStopGBA = true;//请求停止当前正在进行的全局BA
        // 记录全局BA次数
        mnFullBAIdx++;
        if(mpThreadGBA)//std::thread* mpThreadGBA;判断全局BA线程是否正在进行
        {
   
            mpThreadGBA->detach();//关掉全局BA线程
            delete mpThreadGBA;//删除这个全局BA
        }
    }

    while(!mpLocalMapper->isStopped())//局部建图线程结束后，才能进入闭环矫正
    {
   
        //usleep(1000);
        std::this_thread::sleep_for(std::chrono::milliseconds(1));//未结束，重复等待一毫秒，直到结束
    }

1.2 更新当前关键帧与其它关键帧之间的连接关系

 mpCurrentKF->UpdateConnections();

因为闭环检测，以及sim3的计算时可能对多当前关键帧删除了一些地图点，这样因为这些点的变动导致当前关键帧和其共视关键帧的连接关系发生了变化，所以这里要更新一下当前关键帧的连接关系。函数实现如下：

	map<KeyFrame*,int> KFcounter; // 关键帧-权重，关键帧为共视关键帧，权重为共视关键帧与当前关键帧共视地图点的个数 

    vector<MapPoint*> vpMP;  //定义变量，存储当前关键帧的地图点  
    
    {
   
        // 获得该关键帧的所有3D点
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;  //当前关键帧所有的地图点放进vpMP         
    }
    
    // 统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)//循环当前关键帧的地图点
    {
   
        MapPoint* pMP = *vit;//取出当前循环的当前关键帧的地图点

        if(!pMP)        
            continue;

        if(pMP->isBad()) //该MapPoint已被删除
            continue;

        // observations记录了可以观测到该MapPoint的所有关键帧,<关键帧，在该关键帧中地图点的索引>
        map<KeyFrame*,size_t> observations = pMP->GetObservations();

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(