(01)ORB-SLAM2源码无死角解析-(38) EPnP 源代码分析(1)→PnPsolver总体流程与思路

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析-接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

有兴趣的朋友可以加微信 17575010159 相互讨论技术 - 文末公众号也可关注
 

一、前言

通过前面的博客，相信大家 EPnP 的理论已经有了一些了解，那么接下来就是对其源码进行分析了。在这之前，再来复述一下 EPnP 的逻辑思路:

逻辑思路: 先把2D图像点通过内参变换到相机坐标系下的3D点，然后用ICP来求解3D-3D的变换就得到了位姿。
1

那么很明显，其难点在于如何通过2D信息，加上一些约束，来得到相机坐标系下的3D点。这里使用到的为：因为位姿变换是欧式空间下的刚体变换，所以点之间的相对距离信息在不同坐标系(世界坐标系与相机坐标系)下是不变的。使用 EPnP 求解需要已知:
       $(1):$ n个3D参考点在世界坐标系中的坐标
       $(2):$ n个3D参考点在相机坐标系下对应的n个2D投影坐标
       $(3):$ 相机内参
瞒住以上已知条件，则可以通过 相机 在世界坐标系下的姿态→也就是相机姿态。下面就来对源码进行解析吧，主要分两个阶段进行讲解，先讲总结流程与思路，再核心函数进行细节分析。
 

二、初始化函数

从前面博客中，可以得知函数 Tracking::Relocalization() 调用了 EPnP 函数，其主要代码位于 src/Tracking 中，主要部分如下：

bool Tracking::Relocalization()
{
	PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
	pSolver->SetRansacParameters(
	    0.99,   //用于计算RANSAC迭代次数理论值的概率
	    10,     //最小内点数, 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个
	    300,    //最大迭代次数
	    4,      //最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中
	    0.5,    //这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的
	    5.991); // 自由度为2的卡方检验的阈值,程序中还会根据特征点所在的图层对这个阈值进行缩放
	vpPnPsolvers[i] = pSolver;

    PnPsolver* pSolver = vpPnPsolvers[i];
    cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

从源码的 Relocalization() 中，可以得知，如果当前帧与候选关键帧匹配的数目大于15个，则会创建一个 PnPsolver 类对象(即每一符合条件的关键帧都对应一个 PnPsolver 对象)，存储于 vpPnPsolvers 变量之中。
        创建 PnPsolver 类对象，即初始化函数，其主要是对当前帧与候选关键帧存匹配点对应地图点，进行遍历，如果该地图点不是坏点，则获该地图点在世界坐标系下的3D点，与对应关键帧的2维特征点，以及相机内参等。
        宁外还默认的RANSAC参数，如用于计算RANSAC理论迭代次数所用的概率probability;退出RANSAC所需要的最小内点个数(注意这个只是给定值,最终迭代的时候不一定按照这个来minInliers);设定的最大RANSAC迭代次数maxIterations;表示求解这个问题所需要的最小的样本数目(简称最小集,参与到最小内点数的确定过程中,默认是4)等，代码注释如下：

// 在大体的pipeline上和Sim3Solver差不多,都是 构造->设置RANSAC参数->外部调用迭代函数,进行计算->得到计算的结果

// pcs表示3D点在camera坐标系下的坐标
// pws表示3D点在世界坐标系下的坐标
// us表示图像坐标系下的2D点坐标
// alphas为真实3D点用4个虚拟控制点表达时的系数
// 构造函数
PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):
    pws(0), us(0), alphas(0), pcs(0), //这里的四个变量都是指针啊,直接这样子写的原因可以参考函数 set_maximum_number_of_correspondences()
    maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),
    mnIterations(0), mnBestInliers(0), N(0)
{
    // 根据点数初始化容器的大小
    mvpMapPointMatches = vpMapPointMatches;           //匹配关系
    mvP2D.reserve(F.mvpMapPoints.size());             //2D特征点
    mvSigma2.reserve(F.mvpMapPoints.size());          //特征点金字塔层级
    mvP3Dw.reserve(F.mvpMapPoints.size());            //世界坐标系下的3D点
    mvKeyPointIndices.reserve(F.mvpMapPoints.size()); //记录被使用特征点在原始特征点容器中的索引，因为有些3D点不一定存在，所以索引是不连续的
    mvAllIndices.reserve(F.mvpMapPoints.size());      //记录被使用特征点的索引，是连续的

    // 生成地图点、对应2D特征点，记录一些索引坐标
    int idx=0;
    // 遍历给出的每一个地图点
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
        MapPoint* pMP = vpMapPointMatches[i];//依次获取每个地图点

        if(pMP)
        {
            if(!pMP->isBad())
            {
                const cv::KeyPoint &kp = F.mvKeysUn[i];//得到2维特征点, 将KeyPoint类型变为Point2f

                mvP2D.push_back(kp.pt);   //存放2维特征点
                mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);   //记录特征点是在哪一层提取出来的

                cv::Mat Pos = pMP->GetWorldPos();   //世界坐标系下的3D点
                mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0),Pos.at<float>(1), Pos.at<float>(2)));

                mvKeyPointIndices.push_back(i); //记录被使用特征点在原始特征点容器中的索引, mvKeyPointIndices是跳跃的
                mvAllIndices.push_back(idx);    //记录被使用特征点的索引, mvAllIndices是连续的

                idx++;
            }
        }
    } // 遍历给出的每一个地图点

    // Set camera calibration parameters
    fu = F.fx;
    fv = F.fy;
    uc = F.cx;
    vc = F.cy;

    // 设置默认的RANSAC参数,这个和Sim3Solver中的操作是相同的
    SetRansacParameters();
}


/**
 * @brief 设置RANSAC迭代的参数
 * @param[in] probability       用于计算RANSAC理论迭代次数所用的概率
 * @param[in] minInliers        退出RANSAC所需要的最小内点个数, 注意这个只是给定值,最终迭代的时候不一定按照这个来
 * @param[in] maxIterations     设定的最大RANSAC迭代次数
 * @param[in] minSet            表示求解这个问题所需要的最小的样本数目,简称最小集;参与到最小内点数的确定过程中，默认是4
 * @param[in] epsilon           希望得到的 内点数/总体数 的比值,参与到最小内点数的确定过程中
 * @param[in] th2               内外点判定时的距离的baseline(程序中还会根据特征点所在的图层对这个阈值进行缩放的)
 */
void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
{
    // 注意这次里在每一次采样的过程中,需要采样四个点,即最小集应该设置为4

    // Step 1 获取给定的参数
    mRansacProb = probability;
    mRansacMinInliers = minInliers;
    mRansacMaxIts = maxIterations;
    mRansacEpsilon = epsilon;         
    mRansacMinSet = minSet;           


    // Step 2 计算理论内点数,并且选 min(给定内点数,最小集,理论内点数) 作为最终在迭代过程中使用的最小内点数
    N = mvP2D.size(); // number of correspondences, 所有二维特征点个数

    mvbInliersi.resize(N);// inlier index, mvbInliersi记录每次迭代inlier的点

    // Adjust Parameters according to number of correspondences
    // 再根据 epsilon 来计算理论上的内点数;
    // NOTICE 实际在计算的过程中使用的 mRansacMinInliers = min(给定内点数,最小集,理论内点数)
    int nMinInliers = N*mRansacEpsilon; 
    if(nMinInliers<mRansacMinInliers)
        nMinInliers=mRansacMinInliers;
    if(nMinInliers<minSet)
        nMinInliers=minSet;
    mRansacMinInliers = nMinInliers;

    // Step 3 根据敲定的"最小内点数"来调整 内点数/总体数 这个比例 epsilon

    // 这个变量却是希望取得高一点,也可以理解为想让和调整之后的内点数 mRansacMinInliers 保持一致吧
    if(mRansacEpsilon<(float)mRansacMinInliers/N)
        mRansacEpsilon=(float)mRansacMinInliers/N;

    // Step 4  根据给出的各种参数计算RANSAC的理论迭代次数,并且敲定最终在迭代过程中使用的RANSAC最大迭代次数
    // Set RANSAC iterations according to probability, epsilon, and max iterations -- 这个部分和Sim3Solver中的操作是一样的
    int nIterations;

    if(mRansacMinInliers==N)//根据期望的残差大小来计算RANSAC需要迭代的次数
        nIterations=1;
    else
        nIterations = ceil(log(1-mRansacProb)/log(1-pow(mRansacEpsilon,3)));

    mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));

    // Step 5 计算不同图层上的特征点在进行内点检验的时候,所使用的不同判断误差阈值

    mvMaxError.resize(mvSigma2.size());// 图像提取特征的时候尺度层数
    for(size_t i=0; i<mvSigma2.size(); i++)// 不同的尺度，设置不同的最大偏差
        mvMaxError[i] = mvSigma2[i]*th2;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

 

三、循环迭代 iterate

如果当前帧与候选关键帧匹配特征点大于15个，则对其进行之态估算(迭代优化)。函数实现于 PnPsolver.cc 文件 中，代码思路：

$(1):$ 进入while循环，条件一，历史进行的迭代次数少于最大迭代值。条件二，当前进行的迭代次数少于当前函数给定的最大迭代值。当两个条件都=不满足时跳出循环。

$(2):$ 随机选取 mRansacMinSet(默认为4) 对 3D-2D 数据，并且赋值给 pws(世界坐标系下3D坐标)，us(3D坐标在相机坐标系下对应的2D点)

$(3):$ 经过compute_pose(mRi, mti); 计算相机位姿，其为EPnP 的核心部分。

$(4):$ 根据计算出来的相机位姿，将3D点由世界坐标系旋转到相机坐标系，然后进行针孔投影到2D，与关键点计算误差，判断是否为内点。

$(5):$ 如果当前次迭代得到的内点数已经达到了合格的要求了，更新最佳的计算结果。并且用新的内点来继续对位姿进行精求解。

其代码注释如下：

/**
 * @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();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

 

四、结语

通过上面的介绍，相信大家对于 EPnP 的总体流程应该是是否了解了，那么下一部就是要进行细致的分析了，也就是其中的函数 compute_pose(mRi, mti); 进行讲解。