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

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

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

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

那么很明显，其难点在于如何通过2D信息，加上一些约束，来得到相机坐标系下的3D点。这里使用到的为：因为位姿变换是欧式空间下的刚体变换，所以点之间的相对距离信息在不同坐标系(世界坐标系与相机坐标系)下是不变的。使用 EPnP 求解需要已知:        ( 1 ) : \color{blue}{(1)}: (1): n个3D参考点在世界坐标系中的坐标        ( 2 ) : \color{blue}{(2)}: (2): n个3D参考点在相机坐标系下对应的n个2D投影坐标        ( 3 ) : \color{blue}{(3)}: (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);
}

从源码的 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 &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(); iisBad())
            {
                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(0),Pos.at(1), Pos.at(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