ORB-SLAM2 --- LoopClosing::ComputeSim3 函数

目录

1.什么是sim3，为什么要做sim3

2.函数流程 

3.code 

4.函数解析 

4.1 准备工作

4.2 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver 

4.3 对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败 

4.4 取出与当前帧闭环匹配上的关键帧及其共视关键帧，以及这些共视关键帧的地图点 

4.5 将闭环关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配 

4.6 统计当前帧与闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败 

---

1.什么是sim3，为什么要做sim3

        用三对点求取相似变换。

         为什么需要计算Sim3？ 均摊误差、自然过渡。

        当前关键帧、闭环关键帧之间其实是隔了很多帧的，他们的位姿信息都是由邻近信息得到的，经过了很久后，很可能有累计的位姿误差（单目尺度漂移所以有尺度误差，双目或RGBD认为没有尺度误差）， 如果你闭环时直接根据各自的位姿强制把相隔了很久的两个位姿接上，很可能会导致明显的错位。 我们用扫描人脸来做个比喻，可以认为开始扫描的脸和最后闭环的脸之间你是直接强制缝合，那很可能两张脸接不上，五官错位。

        用SIM3就是把相隔很久的两个要缝合的关键帧（及其周围关键帧）重新建立连接，做一个软过渡， 尽可能的将累计误差分摊到要缝合的关键帧（及其周围关键帧），也就是闭环调整。相当于我们把要缝合的人脸两侧都做了一定的调整使得缝合不那么生硬，起码看起来更协调。

2.函数流程 

Sim3 计算流程说明：
        1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧---闭环帧）          
        2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧---闭环帧）   
        3. 将闭环帧以及闭环帧相连的关键帧的地图点与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧）     
        注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3
        对于双目或者是RGBD输入的情况。计算得到的尺度=1。

3.code 

bool LoopClosing::ComputeSim3()
{
    // Sim3 计算流程说明：
    // 1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧---闭环帧）          
    // 2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧---闭环帧）   
    // 3. 将闭环帧以及闭环帧相连的关键帧的地图点与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧）     
    // 注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3
    // 对于双目或者是RGBD输入的情况,计算得到的尺度=1


    //  准备工作
    // 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
    ORBmatcher matcher(0.75,true);

    // 存储每一个候选帧的Sim3Solver求解器
    vector vpSim3Solvers;
    vpSim3Solvers.resize(nInitialCandidates);

    // 存储每个候选帧的匹配地图点信息
    vector > vvpMapPointMatches;
    vvpMapPointMatches.resize(nInitialCandidates);

    // 存储每个候选帧应该被放弃(True）或者 保留(False)
    vector vbDiscarded;
    vbDiscarded.resize(nInitialCandidates);

    // 完成 Step 1 的匹配后，被保留的候选帧数量
    int nCandidates=0;

    // Step 1. 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
    for(int i=0; iSetNotErase();

        // 如果候选帧质量不高，直接PASS
        if(pKF->isBad())
        {
            vbDiscarded[i] = true;
            continue;
        }

        // Step 1.2 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
        // 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
        // vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环帧
        int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

        // 粗筛：匹配的特征点数太少，该候选帧剔除
        if(nmatches<20)
        {
            vbDiscarded[i] = true;
            continue;
        }
        else
        {
            // Step 1.3 为保留的候选帧构造Sim3求解器
            // 如果 mbFixScale（是否固定尺度） 为 true，则是6 自由度优化（双目 RGBD）
            // 如果是false，则是7 自由度优化（单目）
            Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);

            // Sim3Solver Ransac 过程置信度0.99，至少20个inliers 最多300次迭代
            pSolver->SetRansacParameters(0.99,20,300);
            vpSim3Solvers[i] = pSolver;
        }

        // 保留的候选帧数量
        nCandidates++;
    }

    // 用于标记是否有一个候选帧通过Sim3Solver的求解与优化
    bool bMatch = false;

    // Step 2 对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
    while(nCandidates>0 && !bMatch)
    {
        // 遍历每一个候选帧
        for(int i=0; i vbInliers; 
        
            // 内点（Inliers）数量
            int nInliers;

            // 是否到达了最优解
            bool bNoMore;

            // Step 2.1 取出从 Step 1.3 中为当前候选帧构建的 Sim3Solver 并开始迭代
            Sim3Solver* pSolver = vpSim3Solvers[i];

            // 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
            cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

            // If Ransac reachs max. iterations discard keyframe
            // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
            // 如果计算出了Sim3变换，继续匹配出更多点并优化。因为之前 SearchByBoW 匹配可能会有遗漏
            if(!Scm.empty())
            {
                // 取出经过Sim3Solver 后匹配点中的内点集合
                vector vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast(NULL));
                for(size_t j=0, jend=vbInliers.size(); jGetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();

                // 查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用SearchByBoW进行特征点匹配时会有漏匹配）
                // 通过Sim3变换，投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配
                // 只有互相都成功匹配的才认为是可靠的匹配
                matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

                // Step 2.3 用新的匹配来优化 Sim3，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                // OpenCV的Mat矩阵转成Eigen的Matrix类型
                // gScm：候选关键帧到当前帧的Sim3变换
                g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
            
                // 如果mbFixScale为true，则是6 自由度优化（双目 RGBD），如果是false，则是7 自由度优化（单目）
                // 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

                // 如果优化成功，则停止while循环遍历闭环候选
                if(nInliers>=20)
                {
                    // 为True时将不再进入 while循环
                    bMatch = true;
                    // mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
                    mpMatchedKF = pKF;

                    // gSmw：从世界坐标系 w 到该候选帧 m 的Sim3变换，都在一个坐标系下，所以尺度 Scale=1
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);

                    // 得到g2o优化后从世界坐标系到当前帧的Sim3变换
                    mg2oScw = gScm*gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
                    mvpCurrentMatchedPoints = vpMapPointMatches;

                    // 只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                    break;
                }
            }
        }
    }

    // 退出上面while循环的原因有两种,一种是求解到了bMatch置位后出的,另外一种是nCandidates耗尽为0
    if(!bMatch)
    {
        // 如果没有一个闭环匹配候选帧通过Sim3的求解与优化
        // 清空mvpEnoughConsistentCandidates，这些候选关键帧以后都不会在再参加回环检测过程了
        for(int i=0; iSetErase();
        // 当前关键帧也将不会再参加回环检测了
        mpCurrentKF->SetErase();
        // Sim3 计算失败，退出了
        return false;
    }

    // Step 3：取出与当前帧闭环匹配上的关键帧及其共视关键帧，以及这些共视关键帧的地图点
    // 注意是闭环检测出来与当前帧形成闭环的关键帧 mpMatchedKF
    // 将mpMatchedKF共视的关键帧全部取出来放入 vpLoopConnectedKFs
    // 将vpLoopConnectedKFs的地图点取出来放入mvpLoopMapPoints
    vector vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();

    // 包含闭环匹配关键帧本身,形成一个“闭环关键帧小组“
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    mvpLoopMapPoints.clear();

    // 遍历这个组中的每一个关键帧
    for(vector::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
    {
        KeyFrame* pKF = *vit;
        vector vpMapPoints = pKF->GetMapPointMatches();

        // 遍历其中一个关键帧的所有有效地图点
        for(size_t i=0, iend=vpMapPoints.size(); iisBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
                {
                    mvpLoopMapPoints.push_back(pMP);
                    // 标记一下
                    pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                }
            }
        }
    }

    // Find more matches projecting with the computed Sim3
    // Step 4：将闭环关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配
    // 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
    // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配对
    // mvpCurrentMatchedPoints是前面经过SearchBySim3得到的已经匹配的点对，这里就忽略不再匹配了
    // 搜索范围系数为10
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

    // If enough matches accept Loop
    // Step 5: 统计当前帧与闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败
    int nTotalMatches = 0;
    for(size_t i=0; i=40)
    {
        // 如果当前回环可靠,保留当前待闭环关键帧，其他闭环候选全部删掉以后不用了
        for(int i=0; iSetErase();
        return true;
    }
    else   
    {
        // 闭环不可靠，闭环候选及当前待闭环帧全部删除
        for(int i=0; iSetErase();
        mpCurrentKF->SetErase();
        return false;
    }
}

4.函数解析 

4.1 准备工作

        对每个（上一步得到的具有足够连续关系的）闭环候选帧（存储在mvpEnoughConsistentCandidates容器中）都准备算一个Sim3。

        用nInitialCandidates变量保存在DetectLoop中计算出来的回环候选关键帧的数量。

        用vpSim3Solvers容器存储每个闭环候选关键帧的Sim3Solver求解器。

        用vvpMapPointMatches变量（vector >）存储每个每个候选帧的匹配地图点信息。

        用vbDiscarded存储每个候选帧应该被放弃(True）或者 保留(False)。

4.2 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver 

        遍历每个闭环候选关键帧，为其设置SetNotErase选项避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除，如果候选帧质量不高，直接PASS。

        通过BoW匹配匹配当前帧 mpCurrentKF 与闭环候选关键帧的特征点匹配信息：

ORB-SLAM2 ---- Frame::ComputeBoW函数解析https://blog.csdn.net/qq_41694024/article/details/128007040ORB-SLAM2 ---- ORBmatcher::SearchByBoW函数解析https://blog.csdn.net/qq_41694024/article/details/126322962       

        用nmatches返回SearchByBoW函数成功匹配到的两帧的特征点个数，用vvpMapPointMatches[i]保存特征点匹配的信息。

        粗筛：匹配的特征点数太少，该候选帧剔除：

        ①将此帧的候选关键帧地位废除vbDiscarded[i] = true;。

        若匹配的特征点满足要求：

        ①为该候选关键帧构造Sim3Solver求解器。

        ②将此求解器复制到vpSim3Solvers求解器容器中。

        返回保留的候选帧数量nCandidates。

4.3 对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败 

        我们到这先说几个变量：

        vbDiscarded：候选帧是否被抛弃的向量，我们在4.2节中设置判定条件：如果候选帧质量不高，直接PASS；如果通过BoW粗匹配的特征点数太少，该候选帧剔除。

        bMatch：用于标记是否有一个候选帧通过Sim3Solver的求解与优化

        nCandidates：经过4.2节筛选后的候选帧的数量

        遍历每一个候选帧（DetectLoop传来的mvpEnoughConsistentCandidates容器）：

        判断：
        ①该帧是否被标志为被遗弃vbDiscarded。跳过该闭环候选关键帧。

        取出vpSim3Solvers中的关于该帧的Sim3Solver开始迭代，得到的的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12），若总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除（vbDiscarded设置为true，nCandidates--）。

        如果计算出了Sim3变换，继续匹配出更多点并优化。因为之前 SearchByBoW 匹配可能会有遗漏。利用SearchBySim3函数得到更多特征点匹配：

ORB-SLAM2 --- ORBmatcher::SearchBySim3函数解析https://blog.csdn.net/qq_41694024/article/details/128585315        用新的匹配来优化 Sim3，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断：

        用g2o优化，若优化后的内点数nInliers>=20，优化成功，则停止while循环遍历闭环候选帧，得到最终的闭环候选关键帧mpMatchedKF，得到g2o优化后从世界坐标系到当前帧的Sim3变换mg2oScw、mScw，得到mvpCurrentMatchedPoints是当前帧mpCurrentKF到最终闭环帧mpMatchedKF的特征点匹配信息。

        退出上面while循环的原因有两种，一种是求解到了bMatch置位后出的，另外一种是nCandidates耗尽为0。

        如果bMatch置位为true，则我们找到了与当前帧mpCurrentKF的闭环关键帧。

        如果bMatch置位为false，我们没有找到闭环候选关键帧，将闭环候选关键帧储存的容器mvpEnoughConsistentCandidates置为空，当前关键帧也将不会再参加回环检测了，向上层函数返回false表示闭环失败。

4.4 取出与当前帧闭环匹配上的关键帧及其共视关键帧，以及这些共视关键帧的地图点 

        将mpMatchedKF共视的关键帧和它自己全部取出来放入 vpLoopConnectedKFs 中，将vpLoopConnectedKFs的地图点取出来放入mvpLoopMapPoints中。

4.5 将闭环关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配 

/**
 * @brief 根据Sim3变换，将闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF，生成新的匹配点对
 * 
 * @param[in] pKF               当前KF
 * @param[in] Scw               当前KF和闭环KF之间的Sim3变换
 * @param[in] vpPoints          闭环KF及其共视KF的地图点
 * @param[in] vpMatched         当前KF的已经匹配的地图点
 * @param[in] th                搜索范围
 * @return int                  返回新的成功匹配的点对的数目
 */
int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector &vpPoints, vector &vpMatched, int th)
{
    // Get Calibration Parameters for later projection
    const float &fx = pKF->fx;
    const float &fy = pKF->fy;
    const float &cx = pKF->cx;
    const float &cy = pKF->cy;

    // Decompose Scw
    // Step 1 分解Sim变换矩阵
    // 这里的尺度在Pc归一化时会被约掉。可以理解为投影的时候不需要尺度，因为变换到了射线上，尺度无关
    // 尺度会在后面优化的时候用到
    cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
    const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));   // 计算得到尺度s
    cv::Mat Rcw = sRcw/scw;                                 // 保证旋转矩阵行列式为1
    cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw;             // 去掉尺度后的平移向量
    cv::Mat Ow = -Rcw.t()*tcw;                              // 世界坐标系下相机光心坐标

    // Set of MapPoints already found in the KeyFrame
    // 使用set类型，记录前面已经成功的匹配关系，避免重复匹配。并去除其中无效匹配关系（NULL）
    set spAlreadyFound(vpMatched.begin(), vpMatched.end());
    spAlreadyFound.erase(static_cast(NULL));

    int nmatches=0;

    // For each Candidate MapPoint Project and Match
    // Step 2 遍历闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF
    for(int iMP=0, iendMP=vpPoints.size(); iMPisBad() || spAlreadyFound.count(pMP))
            continue;

        // Get 3D Coords.
        // Step 2.2 投影到当前KF的图像坐标并判断是否有效
        cv::Mat p3Dw = pMP->GetWorldPos();

        // Transform into Camera Coords.
        cv::Mat p3Dc = Rcw*p3Dw+tcw;

        // Depth must be positive
        // 深度值必须为正
        if(p3Dc.at(2)<0.0)
            continue;

        // Project into Image
        const float invz = 1/p3Dc.at(2);
        const float x = p3Dc.at(0)*invz;
        const float y = p3Dc.at(1)*invz;

        const float u = fx*x+cx;
        const float v = fy*y+cy;

        // Point must be inside the image
        // 在图像范围内
        if(!pKF->IsInImage(u,v))
            continue;

        // Depth must be inside the scale invariance region of the point
        // 判断距离是否在有效距离内
        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        // 地图点到相机光心的向量
        cv::Mat PO = p3Dw-Ow;
        const float dist = cv::norm(PO);

        if(distmaxDistance)
            continue;

        // Viewing angle must be less than 60 deg
        // 观察角度小于60°
        cv::Mat Pn = pMP->GetNormal();

        if(PO.dot(Pn)<0.5*dist)
            continue;

        // 根据当前这个地图点距离当前KF光心的距离,预测该点在当前KF中的尺度(图层)
        int nPredictedLevel = pMP->PredictScale(dist,pKF);

        // Search in a radius
        // 根据尺度确定搜索半径
        const float radius = th*pKF->mvScaleFactors[nPredictedLevel];

        //  Step 2.3 搜索候选匹配点
        const vector vIndices = pKF->GetFeaturesInArea(u,v,radius);

        if(vIndices.empty())
            continue;

        // Match to the most similar keypoint in the radius
        const cv::Mat dMP = pMP->GetDescriptor();

        int bestDist = 256;
        int bestIdx = -1;
        //  Step 2.4 遍历候选匹配点，找到最佳匹配点
        for(vector::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
        {
            const size_t idx = *vit;
            if(vpMatched[idx])
                continue;

            const int &kpLevel= pKF->mvKeysUn[idx].octave;

            // 不在一个尺度也不行
            if(kpLevelnPredictedLevel)
                continue;

            const cv::Mat &dKF = pKF->mDescriptors.row(idx);

            const int dist = DescriptorDistance(dMP,dKF);

            if(dist

        这里主要是增加地图点的匹配。

4.6 统计当前帧与闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败 

        如果当前回环可靠,保留当前待闭环关键帧，其他闭环候选全部删掉以后不用了。

        向上层函数返回候选关键帧。