软件工程应用与实践第十二篇

ORB-SLAM3 PnPSolver.cc相关代码分析

2021SC@SDUSC

---

前言

之前对PnPSolver.cc做过一篇代码分析，这是第二篇，会对更细节的代码进行分析

---

一、算法原理及步骤

针对PnP问题，使用了RANSAC, EPnP两种算法共同求解位姿 。为了得到更加准确的值，在RANSAC算法框架下迭代使用EPnP算法，最后获得误差最小的。
1.RANSAC步骤 cv::Mat iterate()
·粗求位姿 double compute_pose()
·求局内点 void CheckInliers()
·根据局内点再求位姿 bool Refine()
2.EPnP步骤
·选取四个控制点
·求解4个控制点系数alphas
·初步求解相机坐标系下的控制点坐标

二、代码分析

EPnP迭代计算

/**
 * @brief EPnP迭代计算
 * 
 * @param[in] nIterations   迭代次数
 * @param[in] bNoMore       达到最大迭代次数的标志
 * @param[in] vbInliers     内点的标记
 * @param[in] nInliers      总共内点数
 * @return cv::Mat          计算出来的位姿
 */
cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
{
    bNoMore = false;        //已经达到最大迭代次数的标志
    vbInliers.clear();
    nInliers=0;             // 当前次迭代时的内点数

    // mRansacMinSet 为每次RANSAC需要的特征点数，默认为4组3D-2D对应点
    set_maximum_number_of_correspondences(mRansacMinSet);

    // 如果已有匹配点数目比要求的内点数目还少，直接退出
    // N为所有2D点的个数, mRansacMinInliers 为正常退出RANSAC迭代过程中最少的inlier数
    if(N<mRansacMinInliers)
    {
        bNoMore = true;
        return cv::Mat();
    }

    // mvAllIndices为所有参与PnP的2D点的索引
    // vAvailableIndices为每次从mvAllIndices中随机挑选mRansacMinSet组3D-2D对应点进行一次RANSAC
    vector<size_t> vAvailableIndices;

    // 当前的迭代次数id
    int nCurrentIterations = 0;

    // 进行迭代的条件:
    // 条件1: 历史进行的迭代次数少于最大迭代值
    // 条件2: 当前进行的迭代次数少于当前函数给定的最大迭代值
    while(mnIterations<mRansacMaxIts || nCurrentIterations<nIterations)
    {
        // 迭代次数更新
        nCurrentIterations++;
        mnIterations++;
        // 清空已有的匹配点的计数,为新的一次迭代作准备
        reset_correspondences();

        vAvailableIndices = mvAllIndices;

        // Get min set of points
        // 随机选取4组（默认数目）最小集合
        for(short i = 0; i < mRansacMinSet; ++i)
        {
            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);

            // 将生成的这个索引映射到给定帧的特征点id
            int idx = vAvailableIndices[randi];

            // 将对应的3D-2D压入到pws和us. 这个过程中需要知道将这些点的信息存储到数组中的哪个位置,这个就由变量 number_of_correspondences 来指示了
            add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);

            // 从"可用索引表"中删除这个已经被使用的点
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        } // 选取最小集

        // Compute camera pose
        // 计算相机的位姿
        compute_pose(mRi, mti);

        // Check inliers
        // 通过之前求解的位姿来进行3D-2D投影，统计内点数目
        CheckInliers();

        // 如果当前次迭代得到的内点数已经达到了合格的要求了
        if(mnInliersi>=mRansacMinInliers)
        {
            // If it is the best solution so far, save it
            // 更新最佳的计算结果
            if(mnInliersi>mnBestInliers)
            {
                mvbBestInliers = mvbInliersi;
                mnBestInliers = mnInliersi;

                cv::Mat Rcw(3,3,CV_64F,mRi);
                cv::Mat tcw(3,1,CV_64F,mti);
                Rcw.convertTo(Rcw,CV_32F);
                tcw.convertTo(tcw,CV_32F);
                mBestTcw = cv::Mat::eye(4,4,CV_32F);
                Rcw.copyTo(mBestTcw.rowRange(0,3).colRange(0,3));
                tcw.copyTo(mBestTcw.rowRange(0,3).col(3));
            } // 更新最佳的计算结果

            // 还要求精
            if(Refine())   // 如果求精成功(即表示求精之后的结果能够满足退出RANSAC迭代的内点数条件了)
            {
                nInliers = mnRefinedInliers;
                // 转录,作为计算结果
                vbInliers = vector<bool>(mvpMapPointMatches.size(),false);
                for(int i=0; i<N; i++)
                {
                    if(mvbRefinedInliers[i])
                        vbInliers[mvKeyPointIndices[i]] = true;
                }

                // 对直接返回了求精之后的相机位姿
                return mRefinedTcw.clone();
            } // 如果求精成功

            // 如果求精之后还是打不到能够RANSAC的结果,那么就继续进行RANSAC迭代了

        } // 如果当前次迭代得到的内点数已经达到了合格的要求了
    } // 迭代

    // 如果执行到这里,说明可能已经超过了上面的两种迭代次数中的一个了
    // 如果是超过了程序中给定的最大迭代次数
    if(mnIterations>=mRansacMaxIts)
    {
        // 没有更多的允许迭代次数了
        bNoMore=true;
        // 但是如果我们目前得到的最好结果看上去还不错的话
        if(mnBestInliers>=mRansacMinInliers)
        {
            // 返回计算结果
            nInliers=mnBestInliers;
            vbInliers = vector<bool>(mvpMapPointMatches.size(),false);
            for(int i=0; i<N; i++)
            {
                if(mvbBestInliers[i])
                    vbInliers[mvKeyPointIndices[i]] = true;
            }
            return mBestTcw.clone();
        }
    }

    // 如果也没有好的计算结果,只好说明迭代失败
    return cv::Mat();
}