ORB-SLAM2源码阅读（四）—LoopClosing线程&SIM3变换

Stu.xian

已于 2022-12-17 17:43:42 修改

阅读量590

点赞数

分类专栏： opencv slam 文章标签：大数据

于 2022-12-17 17:18:46 首次发布

本文链接：https://blog.csdn.net/chuangkeiss/article/details/128343170

版权

slam 同时被 2 个专栏收录

11 篇文章 4 订阅

订阅专栏

opencv

3 篇文章 0 订阅

订阅专栏

LoopClosing线程代码解析

1.DetectLoop() 闭环检测

2.计算当前帧与闭环帧的Sim3变换ComputeSim3（）

看了这么多的代码注释感觉头都晕了，抽空捋一下这个线程的大纲，帮助理解一下。

LoopClosing线程的主要工作内容可以分为两步：第一步就是对当前新到来的关键帧进行闭环检测（Loop Detection），不断地在关键帧数据库中查找是否有相似的与当前关键帧形成闭环；另一个是纠正闭环（Loop Correction），就是若存在回环进行位姿矫正。

1.LoopClosing线程代码解析

LoopClosing线程主代码就是LoopClosing::Run()，它不断地在循环每来一次关键帧就进行一次闭环判断。

/**
 * LoopClosing线程的执行函数
 * 在KeyFrameDataBase中查找与mlpLoopKeyFrameQueue中新加入的关键帧相似的闭环候选帧
 * 主要步骤：
 * 1.闭环检测，获得闭环候选帧；
 * 2.计算sim3,根据sim3的计算值更新地图点的位姿；
 * 3.进行地图点融合和位姿优化；
*/
void LoopClosing::Run()
{
    mbFinished =false;
 
    while(1)
    {
        // Check if there are keyframes in the queue
        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            //检测闭环。在共视关系的关键帧中找到与当前关键帧Bow匹配最低得分minScore，在除去当前帧共视关系的关键帧数据库中，检测闭环候选帧
            if(DetectLoop())
            {
               // Compute similarity transformation [sR|t] 计算相似变换
               // In the stereo/RGBD case s=1
               //计算旋转平移的相似性，也就是相似变换
               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   // 进行闭环校正，执行闭环融合和位姿图优化
                   CorrectLoop();
               }
            }
        }       
 
        ResetIfRequested();
 
        if(CheckFinish())
            break;
 
        usleep(5000);
    }
 
    SetFinish();
}

1.DetectLoop() 闭环检测

闭环连续性检测原理示例

2.计算当前帧与闭环帧的Sim3变换ComputeSim3（）

1.SIM3变换

（若了解可直接跳过）

Sim(3)表示三维空间的相似变换(Similarity Transformation)。计算Sim3实际就是计算这三个参数:旋转R、平移t、尺度因子s.理论来说计算Sim3需要3对不共线的点对即可求解。

感性的我们可以理解为，我们有三对匹配的不共线三维点可以构成两个三角形。我们根据三角形各自的法向量可以得到他们之间的旋转，通过相似三角形面积能够得到尺度，用前面得到的旋转和尺度可以把两个三角形平行放置，通过计算距离可以得到平移。

首先我们通过一个来理解一下：

看起来好像没什么问题，但是实际上我们不会这样使用，因为存在如下问题:
1、这个旋转的结果和选择点的顺序关系密切，我们分别让不同的点做坐标系原点，得到的结果不同。2、这种情况不适用于匹配点大于3个的情况。
因此实际上我们不会使用以上方法。我们通常能够拿到远大于3个的三维匹配点对，我们会使用最小二乘法来得到更稳定、更精确的结果。下面进入正文。

1.计算SIM3平移

假设我们得到了n > 3组匹配的三维点，分别记为{P},{Q}，其中i= 1,...,n我们的目的是对于每对匹配点，找到如下的变换关系:其中s 是尺度因子，R是旋转，t是平移。

但实际上数据是不可避免会有噪音和误差，所以我们转换思路，定义一个误差e;，我们的目的就是寻找合适的尺度因子、旋转和平移，使得它在所有数据上的误差最小.

在开始求解之前，我们先定义两个三维点集合中所有三维点的均值(或者称为质心、重心),
我们对每个三维点P,Q;分别减去均值，得到去中心化后的坐标P',Q;，则有

下面开始推导我们的误差方程:

然后我们记，根据前面的推导可得等式右边中间项：

这样我们前面的误差方程可以化简为:

等式右边的两项都是大于等于O的平方项，并且只有第二项里的to和我们要求的平移t有关，所以当to =0时，我们可以得到平移的最优解t*。

也就是说我们知道了旋转R和尺度s就能根据三维点均值做差得到平移t了。注意这里平移的方向是 $\left\{P_{i}\right\} \rightarrow\left\{Q_{i}\right\}$ .

计算SIM3尺度因子

进一步简化误差函数：

因为R是旋转矩阵所以模长为1，所以，接着再进行化简。

其实这样的二元一次方程解法得到的尺度并不稳定。所以需要重新构造误差函数，使得我们得到的尺度是对称的、稳定的。

上面等式右边第一项只和尺度s有关的平方项，第二项和s无关，但和旋转R有关，因此令第一项为0，我们就能得到最佳的尺度s*

同时，第二项里的Sp,Sg都是平方项，所以令第二项里的最大，可以使得剩下的误差函数最小。

3.计算旋转

其中：

我们定义

其中：

引入M是为了方便用其元素来表示N，我们将上面的结果代入整理，则有:

然后我们对ⅣN进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，注意这里旋转的方向是 $\left\{P_{i}\right\} \rightarrow\left\{Q_{i}\right\}$ 。

2.计算相似变换ComputeSim3

1. 遍历候选闭环关键帧中每个关键帧，对每个候选关键帧和当前关键帧之间求解sim3。

2.获取当前关键帧的匹配闭环关键帧及其相连关键帧对应的地图点。将这些地图点通过上面优化得到的Sim3(位姿mScw) 变换投影到当前关键帧进行匹配SearchByProjection：
如果匹配点数>=40，则判定当前关键帧和闭环候选关键帧存在闭环关系；
如果匹配点数<40，则判定当前关键这暖和闭环候选帧之间没有闭环关系；

/**
 * 计算当前关键帧和闭环候选帧之间的Sim3,这个Sim3变换就是闭环前累计的尺度和位姿误差
 * 该误差也可以帮助检验该闭环在空间几何姿态上是否成立
*/
bool LoopClosing::ComputeSim3()
{
    // For each consistent loop candidate we try to compute a Sim3
    //对每一个连续的闭环候选帧，我们都尝试计算sim3
    //获得候选关键帧的个数
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
 
    // We compute first ORB matches for each candidate
    // If enough matches are found, we setup a Sim3Solver
    // 我们为每一个候选帧计算第一个ORB匹配项
    // 如果足够的匹配项发现了，我们就启动Sim3Solver
    ORBmatcher matcher(0.75,true);
 
    vector<Sim3Solver*> vpSim3Solvers;
    vpSim3Solvers.resize(nInitialCandidates);
 
    vector<vector<MapPoint*> > vvpMapPointMatches;
    vvpMapPointMatches.resize(nInitialCandidates);
 
    vector<bool> vbDiscarded;
    vbDiscarded.resize(nInitialCandidates);
 
    int nCandidates=0; //candidates with enough matches
    //1.遍历候选关键帧，对每一个候选关键帧和当前关键帧之间匹配的特征点进行sim3求解
    for(int i=0; i<nInitialCandidates; i++)
    {
        KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
 
        // avoid that local mapping erase it while it is being processed in this thread
        pKF->SetNotErase();
 
        if(pKF->isBad())
        {
            vbDiscarded[i] = true;
            continue;
        }
        /**
         * 这里主要是通过SearchByBow搜索当前关键帧中和闭环候选帧匹配的地图点
         * BoW通过将单词聚类到树结构node的方法，这样可以加快搜索匹配速度
         * vvpMapPointMatches[i]用于存储当前关键帧和候选关键帧之间匹配的地图点
        */
        int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
        /**
         * 若nmatches<20，剔除该候选帧
         * 注意这里使用Bow匹配较快，但是会有漏匹配
        */
        if(nmatches<20)
        {
            vbDiscarded[i] = true;
            continue;
        }
        else
        {
            //构建Sim3求解器
            Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
            pSolver->SetRansacParameters(0.99,20,300);
            vpSim3Solvers[i] = pSolver;
        }
 
        nCandidates++;
    }
 
    bool bMatch = false;
 
    // Perform alternatively RANSAC iterations for each candidate
    // until one is succesful or all fail
    /**
     * RANSAC：利用上面匹配上的地图点（虽然匹配上了，但是空间位置相差了一个Sim3），用RANSAC方法求解Sim3位姿
     * 这里有可能求解不出Sim3,也就是虽然匹配满足，但是空间几何姿态不满足vvpMapPointMatches
    */
    while(nCandidates>0 && !bMatch)
    {
        for(int i=0; i<nInitialCandidates; i++)
        {
            if(vbDiscarded[i])
                continue;
 
            KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
 
            // Perform 5 Ransac Iterations
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;
 
            Sim3Solver* pSolver = vpSim3Solvers[i];
            //用RANSAC方法求解SIM3，一共迭代五次，可以提高优化结果准确性
            cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
 
            // If Ransac reachs max. iterations discard keyframe
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }
 
            // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
            if(!Scm.empty())
            {
                vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
                for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
                {
                    if(vbInliers[j])
                       vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                }
                //根据计算出的Sim3(s, R, t)，去找更多的匹配点(SearchBySim3),更新vpMapPointMatches
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();
                //使用sim3求出来的s,R,t通过SearchBySim3得到更多匹配
                matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
                /**
                 * 使用更新过的匹配，使用g2o优化Sim3位姿，这是内点数nInliers>20,才说明通过。
                 * 一旦找到闭环帧mpMatchedKF,则break,跳过对其他候选帧的判断
                */
                g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
 
                // If optimization is succesful stop ransacs and continue
                //如果当前帧和回环帧之间存在超过20个匹配点，则认为这个当前帧和回环帧的sim3变换是有效的
                if(nInliers>=20)
                {
                    bMatch = true;
                    mpMatchedKF = pKF;
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
                    mg2oScw = gScm*gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
 
                    mvpCurrentMatchedPoints = vpMapPointMatches;
                    break;
                }
            }
        }
    }
 
    if(!bMatch)
    {
        for(int i=0; i<nInitialCandidates; i++)
             mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
 
    // Retrieve MapPoints seen in Loop Keyframe and neighbors
    //将MatchedKF共视帧取出
    vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    //匹配的关键帧mpMatchedKF加入到和它共视的关键帧列表中
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    mvpLoopMapPoints.clear();
    for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
    {
        KeyFrame* pKF = *vit;
        vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                if(!pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId)
                {
                    mvpLoopMapPoints.push_back(pMP);
                    pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                }
            }
        }
    }
 
    // Find more matches projecting with the computed Sim3
    /**
     * 获取mpMatchedKF及其相连关键帧对应的地图的地图点。将这些地图点通过上面优化得到的Sim3(gScm>mScw) 
     * 变换投影到当前关键帧进行匹配，若匹配点>=40个，则返回true,进行闭环调整，否则，返回false, 
     * 线程暂停5ms后继续接收Tracking发送来的关键帧队列 
     * 注意这里得到的当前关键帧中匹配上闭环关键帧共视地图点(mvpCurrentMatchedPoints)
     * 将用于后面CorrectLoop时当时关键帧地图点的冲突融合
     * 到这里，不仅确保了当前关键帧与闭环帧之间匹配度高， 
     * 而且保证了闭环帧的共视图中的地图点和当前帧的特征点匹配度更高,说明该闭环帧是正确的
    */
   //SearchByProjection得到更多匹配点，mvpCurrentMatchedPoints
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
 
    // If enough matches accept Loop
    int nTotalMatches = 0;
    for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
    {
        if(mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }
    // 如果最终的匹配点超过40个，则认为当前帧和闭环帧确实存在闭环关系
    if(nTotalMatches>=40)
    {
        for(int i=0; i<nInitialCandidates; i++)
            if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                mvpEnoughConsistentCandidates[i]->SetErase();
        return true;
    }
    else
    {
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
 
}

矫正回环CorrectLoop（未完）

Stu.xian

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
ORB-SLAM2源码阅读（四）—LoopClosing线程&SIM3变换

看了这么多的代码注释感觉头都晕了，抽空捋一下这个线程的大纲，帮助理解一下。LoopClosing线程的主要工作内容可以分为两步：第一步就是对当前新到来的关键帧进行闭环检测（Loop Detection），不断地在关键帧数据库中查找是否有相似的与当前关键帧形成闭环；另一个是纠正闭环（Loop Correction），就是若存在回环进行位姿矫正。
复制链接

扫一扫