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

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(); mit!=mend; mit++)
        {
            /// 在nNextID的基础上加1就得到了mnID，为当前KeyFrame的ID号 long unsigned int mnId;mnId是当前关键帧的id，也就是说自己和自己不算共视
            if(mit->first->mnId==mnId)
                continue;
            KFcounter[mit->first]++;//这行代码表达这么一个意思：能看到这个地图点的关键帧（mit->first），如果这个关键帧不是自身，那么就代表这个点还被其他关键帧看到了，看到了这个点，这个其他关键帧里这个地图点权重就＋1；KFcounter[KeyFrame]就是其他关键帧看到的地图点数，而遍历的地图点都是当前关键帧看到的，所以遍历完成之后KFcounter[KeyFrame]就代表了其他关键帧与当前关键帧的共视程度。看下图
        }   
    }

    // This should not happen 
    if(KFcounter.empty())//判断是否为空，为空就不进行后面了
        return;

共视程度统计出来之后，肯定是与之前不一样的，因为之前因为求解和优化sim3删除了一些地图点。所以下面根据上面统计出来的新的共视程度，更新个关键帧之间的连接关系。

//  找到对应权重最大的关键帧（共视程度最高的关键帧）
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second>nmax)//second就是刚才累加的共视点的数目，统计最大的数目
        {
            nmax=mit->second;
            pKFmax=mit->first;//记录一下最大共视关键帧
        }

        if(mit->second>=th)//如果共视数目大于阈值15，单独拿出来  
        {
            // 对应权重需要大于阈值，对这些关键帧建立连接
            vPairs.push_back(make_pair(mit->second,mit->first));//这里把共视个数放前边，关键帧放在了后面，后面方便根据共视个数排序
            // 对方关键帧也要添加这个信息（共视关键帧添加当前关键帧的连接关系）
            (mit->first)->AddConnection(this,mit->second);//这个this指的是当前关键帧，也就是前面 mpCurrentKF->UpdateConnections()的mpCurrentKF；因为c++类的成员函数中隐藏有代表自己本身的（this）指针。
        }  
    }

    //如果没有大于阈值的关键帧（超过阈值的权重），则对权重最大的关键帧建立连接
	if(vPairs.empty())
    {
        vPairs.push_back(make_pair(nmax,pKFmax));//那就把阈值没超过15的nmax最大权重和对应的最大共视关键帧放进vPairs
        pKFmax->AddConnection(this,nmax);//更新其他关键帧与当前关键帧的连接关系（共视关键帧添加当前关键帧的连接关系，即当前关键帧this作为了pKFmax的共视关键帧）
    }
    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小进行排序,根据权重建立连接关系
    sort(vPairs.begin(),vPairs.end()); 
    // 由于sort默认从小到大排序，所以下面给他们逆序
    list<KeyFrame*> lKFs;    //新定义了2个list，分别放sort后的关键帧和权重    
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }
    
	{
        unique_lock<mutex> lockCon(mMutexConnections);
        // 更新当前帧与其它关键帧的连接权重
        mConnectedKeyFrameWeights = KFcounter;//存当前关键帧与所有的共视关键帧的连接关系，没排序的
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); //存排序后的当前关键帧与共视关键帧的连接关系（权重从大到小对应的共视关键帧）
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());//存排序后的当前关键帧与共视关键帧的连接关系对应的权重(权重从大到小)

        //更新生成树的连接
        if(mbFirstConnection && mnId!=0)//mbFirstConnection第一次创建连接关系，且当前关键帧，mnId!=0，不是第0帧的话，就需要更新当前关键帧的父子关系
        {
            // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent = mvpOrderedConnectedKeyFrames.front();//前面的排序，共视关系最高的在最前面
            // 建立双向连接关系，将当前关键帧作为其子关键帧
            mpParent->AddChild(this);//当前关键帧（UpdateConnections作用的关键帧）作为共视程度最高的共视关键帧的子关键帧   
            mbFirstConnection = false;//代表下一次就不设置父子关系了
        } 
    }

1.3 得到Sim3优化后，通过位姿传播，修正与当前关键帧相连的关键帧位姿及其地图点

其实位姿传播很简单，就是通过前面计算的世界坐标系到当前关键帧的sim3变换（①）和当前关键帧和其共视帧的相对变换（②）得到当前关键帧的共视帧们到世界坐标系的sim3变换（⑤–③的逆），然后和没有矫正的sim3（④）形成闭环，来进行位姿传播和地图点调整，最后再更新一下连接关系。

构建当前关键帧组，记录当前关键帧和世界坐标系之间的sim3变换（整个传播的基石）

    // 取出当前关键帧及其共视关键帧，称为“当前关键帧组”
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);

    // CorrectedSim3：存放闭环g2o优化后当前关键帧的共视关键帧的世界坐标系下Sim3 变换  （矫正的当前关键帧及其sim3也在这里）
    // NonCorrectedSim3：存放没有矫正的当前关键帧的共视关键帧的世界坐标系下Sim3 变换
    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
    
    CorrectedSim3[mpCurrentKF]=mg2oScw;//把当前关键帧和对应的sim3（computesim3得到的世界坐标系到当前关键帧的相机坐标系的sim3变换）放到CorrectedSim3，认为这个是准确的，以它为根基，传播到它的共视关键帧
   
    // 当前关键帧到世界坐标系下的变换矩阵
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();

完成位姿位姿传播（①->③）

        // Step 2.1：通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿（还没有修正）
        // 遍历"当前关键帧组""
        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在ComputeSim3里计算过了
            {
                // 得到当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的相对变换
                cv::Mat Tic = Tiw*Twc;//当前关键帧到当前循环的共视关键帧的位姿变换 = 世界坐标系到当前循环的共视关键帧的相机坐标系下的位姿变换 * 当前关键帧的相机坐标系到世界坐标系下的位姿变换
                cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);//当前关键帧到当前循环的共视关键帧的旋转矩阵R
                cv::Mat tic = Tic.rowRange(0,3).col(3);//当前关键帧到当前循环的共视关键帧的平移向量t

                // g2oSic：当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换
                // 这里是non-correct, 所以scale=1.0
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);//构建当前关键帧到当前循环的共视关键帧的sim3，因为认为离得很近，认为没有尺度漂移，所以尺度为1
                // 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
                g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;//世界坐标系到当前关键帧的共视关键帧的相机坐标系sim3变换 = 当前关键帧到当前循环的共视关键帧的相机坐标系sim3变换 * 世界坐标系到当前关键帧的相机坐标系sim3变换（认为这个是准的，以它为根基传播到世界到共视关键帧的变换）
                // Pose corrected with the Sim3 of the loop closure
                // 存放闭环g2o优化后当前关键帧的共视关键帧的Sim3 位姿
                CorrectedSim3[pKFi]=g2oCorrectedSiw;//把当前循环的共视关键帧和对应的世界坐标系到当前循环的共视关键帧的sim3放到容器CorrectedSim3  //注：这里是存放的矫正的，因为世界到当前的sim3，尺度计算了，传播到共视时，经过当前关键帧的sim3，相当于世界到共视的尺度也是矫正的  
            }

            cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);//世界坐标系到当前循环的共视关键帧的相机坐标系的旋转矩阵R
            cv::Mat tiw = Tiw.rowRange(0,3).col(3);       //世界坐标系到当前循环的共视关键帧的相机坐标系的平移向量t
            g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);//构建世界坐标系到当前关键帧的当前循环的共视关键帧的sim3（直接从世界到共视得到的sim3，未经过当前关键帧的sim3）
            // Pose without correction
            // 存放没有矫正的当前关键帧的共视关键帧的Sim3变换
            NonCorrectedSim3[pKFi]=g2oSiw;//把当前循环的共视关键帧和对应的世界坐标系到当前循环的共视关键帧的sim3放到容器NonCorrectedSim3  //注：这里存放的未矫正的，这里世界坐标系到共视关键帧的sim3，尺度为1，并没有经过当前关键帧的sim3，因为当前关键帧的sim3，我们认为是准的，所以这里尺度是不准的，还未矫正
        }

地图点的修正（只贴核心代码：注意这里遍历的地图点是当前关键帧的共视帧们的地图点，图中荧光绿那个圈）
其实每个地图点的矫正就是经过以下一行代码完成矫正的。（图中④->⑤的过程）

Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));

详细过程如下：

cv::Mat P3Dw = pMPi->GetWorldPos();//当前关键帧的共视帧地图点（未矫正）
Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);//Mat类型转成Eigen类型

 // 关键代码：未矫正的地图点eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ world，得到矫正后的地图点eigCorrectedP3Dw
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));

cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);//把矫正后的地图点Eigen类型转换成Mat类型
pMPi->SetWorldPos(cvCorrectedP3Dw);//设置已矫正地图点

pMPi->mnCorrectedByKF = mpCurrentKF->mnId;// 标记，防止重复矫正

pMPi->mnCorrectedReference = pKFi->mnId;//记录该地图点所在的当前关键帧的共视关键帧id
 
pMPi->UpdateNormalAndDepth();// 因为地图点更新了，需要更新其平均观测方向以及观测距离范围

将矫正后的每个关键帧位姿去除尺度影响，将sim3转化为SE3，完成尺度修正

Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); //取出 世界坐标系到当前关键帧的共视关键帧的相机坐标系sim3变换的旋转矩阵R，并作归一化
Eigen::Vector3d eigt = g2oCorrectedSiw.translation();                 //取出 世界坐标系到当前关键帧的共视关键帧的相机坐标系sim3变换的平移向量t            
double s = g2oCorrectedSiw.scale();                                   //取出 世界坐标系到当前关键帧的共视关键帧的相机坐标系sim3变换的尺度s
// 平移向量中包含有尺度信息，还需要用尺度归一化
eigt *=(1./s);                                                       

cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);                 //转换成李群SE3
// 设置矫正后的新的pose
pKFi->SetPose(correctedTiw);                                        

// 地图点的位置改变了,可能会引起共视关系\权值的改变 
pKFi->UpdateConnections(); 

到此为止，已经完成了闭环后当前关键帧组中的所有地图点的修正了，但是ORBSLAM还多做了一步：他认为通过sim3修正后的地图点的准确度是不如之前闭环关键帧组投影到当前关键帧的匹配点mvpCurrentMatchedPoints准的，所以进行了特征点的替换

		for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++) //循环闭环关键帧组投影到当前关键帧的匹配点
        {
            if(mvpCurrentMatchedPoints[i])//若当前循环的地图点存在
            {
                MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];//取出当前循环的闭环关键帧组投影到当前关键帧的匹配点（认为这个通过sim3优化得到的更准）
                MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);//这里的地图点虽然矫正了，但是他是原本跟踪来的，这可能不如回环匹配来的准。
                if(pCurMP)
                    // 如果有重复的MapPoint，则用匹配的地图点代替现有的
                    // 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差
                    pCurMP->Replace(pLoopMP);//就用经过sim3优化后的闭环关键帧组投影到当前关键帧的地图点 替换 当前关键帧本身的地图点
                else  
                {
                    // 如果当前帧没有该MapPoint，则直接添加
                    mpCurrentKF->AddMapPoint(pLoopMP,i);//添加当前关键帧的i特征点观测到地图点
                    pLoopMP->AddObservation(mpCurrentKF,i);//添加地图点被当前关键帧的i特征点观测
                    pLoopMP->ComputeDistinctiveDescriptors();//从众多观测到该MapPoint的特征点中挑选区分度最高的描述子 //计算最优秀的描述子
                }
            }
        } 

2 非常感谢您的阅读！

3 期待您加入

也非常期待您能关注我的微信公众号–“过千帆”，里面不仅有技术文章还有我的读书分享，希望您在那里也有收获。我们一起进步。