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

宛如新生

已于 2022-11-24 00:03:03 修改

阅读量1.9k

点赞数

分类专栏：视觉SLAM疑难杂症文章标签：人工智能

于 2022-01-12 12:24:00 首次发布

本文链接：https://blog.csdn.net/Walking_roll/article/details/122437130

版权

本文介绍了SLAM中的闭环矫正流程，包括矫正前的准备，如停止局部建图和全局优化线程；更新关键帧间的连接关系因地图点变动而变化；以及利用Sim3优化后的位姿传播修正关键帧位姿和地图点。通过对关键帧的Sim3变换和相对变换，完成位姿传播和地图点矫正，确保闭环优化的准确性。

摘要由CSDN通过智能技术生成

前面介绍了闭环关键帧是如何确定的，包括闭环候选关键帧的层层选拨、通过词袋匹配估计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