EPNP：已知4组（默认）3D-2D匹配点，构建参考点，通过计算参考点的相机坐标，线性组合成路标点的相机坐标。然后使用ICP估计相机间的位姿变换。
需要注意的事，EPNP可以同时使用N组路标点构建M矩阵，然后求解出N个路标点的相机坐标。
使用RANSCA+EPNP的方法估计相机位姿，大致步骤为：
（1）设置RANSAC参数，比如迭代次数、最少内点数。
（2）随机选出4组3D-2D点对，使用EPNP估计路标点的相机坐标。
（3）使用ICP估计当前帧位姿。
（4）计算内点数（重投影误差小于阈值的话即为内点），选内点数最多时对应的位姿为最优位姿。
（5）以内点为一组，再次进行EPNP+ICP估计相机位姿，计算内点。如果内点数满足要求，则获得了最优位姿。
在关键帧模式（以上一帧位姿为当前帧位姿）、恒速运动模型（根据前两帧间的位姿变换估计当前帧位姿）中不会用到EPNP（因为不需要去估计位姿，只需要去优化）。在重定位、回环检测的时候需要使用EPNP估计当前帧位姿，然后进行优化。

构造函数

PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):

（1）保存每一个路标点、特征点的信息。
（2）设置相机内参
（3）设置RANSAC参数。

设置RANSAC参数

/*** @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)

设定RANSAC的参数

计算RANSAC理论次数的所用的概率、退出RANSAC的最少内点数、RANSAC的最大迭代次数等

确定退出RANSAC所需的最小内点数

    int nMinInliers = N*mRansacEpsilon; if(nMinInliers<mRansacMinInliers)nMinInliers=mRansacMinInliers;if(nMinInliers<minSet)nMinInliers=minSet;mRansacMinInliers = nMinInliers;

确定迭代次数

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

计算四个控制点坐标

void PnPsolver::choose_control_points(void)

cws[4][3]：控制点坐标。第一维表示是哪个控制点，第二维表示是哪个坐标(x,y,z)。

第一个控制点坐标为所有点坐标之和的均值

  cws[0][0] = cws[0][1] = cws[0][2] = 0;// 遍历每个匹配点中世界坐标系3D点，然后对每个坐标轴加和// number_of_correspondences 默认是 4for(int i = 0; i < number_of_correspondences; i++)for(int j = 0; j < 3; j++)cws[0][j] += pws[3 * i + j];// 再对每个轴上取均值for(int j = 0; j < 3; j++)cws[0][j] /= number_of_correspondences;

计算其余三个控制点的坐标

  // 将所有的3D参考点写成矩阵，(number_of_correspondences * 3)的矩阵CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);double pw0tpw0[3 * 3], dc[3], uct[3 * 3];         // 下面变量的数据区CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);     // PW0^T * PW0，为了进行特征值分解CvMat DC      = cvMat(3, 1, CV_64F, dc);          // 特征值CvMat UCt     = cvMat(3, 3, CV_64F, uct);         // 特征向量// Step 2.1：将存在pws中的参考3D点减去第一个控制点(均值中心)的坐标（相当于把第一个控制点作为原点）, 并存入PW0for(int i = 0; i < number_of_correspondences; i++)for(int j = 0; j < 3; j++)PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];// Step 2.2：利用特征值分解得到三个主方向// PW0^T * PW0// cvMulTransposed(A_src,Res_dst,order, delta=null,scale=1): // Calculates Res=(A-delta)*(A-delta)^T (order=0) or (A-delta)^T*(A-delta) (order=1)cvMulTransposed(PW0, &PW0tPW0, 1);// 这里实际是特征值分解cvSVD(&PW0tPW0,                         // A&DC,                              // W，实际是特征值&UCt,                             // U，实际是特征向量0,                                // VCV_SVD_MODIFY_A | CV_SVD_U_T);    // flagscvReleaseMat(&PW0);// Step 2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量for(int i = 1; i < 4; i++) {// 这里只需要遍历后面3个控制点double k = sqrt(dc[i - 1] / number_of_correspondences);for(int j = 0; j < 3; j++)cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];

计算控制点的线性参数

4个控制点通过线性参数组合成任意路标点：
Pw = a1×C1+a2×C2+a3×C3+a4×C4（a1,a2,a3,a4为参数，C1、C2、C3、C4为参考点坐标）。

void PnPsolver::compute_barycentric_coordinates(void)

计算M矩阵

根据M矩阵，可求出参考点的相机坐标

void PnPsolver::fill_M(CvMat * M,const int row, const double * as, const double u, const double v)

计算L矩阵

void PnPsolver::compute_L_6x10(const double * ut, double * l_6x10)

计算LB = ρ中的雅克比矩阵、ρ-LB

void PnPsolver::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,double betas[4], CvMat * A, CvMat * b)

使用QR分解来求解增量方程

/*** @brief 使用QR分解来求解增量方程 * @param[in]  A   系数矩阵* @param[in]  b   非齐次项* @param[out] X   增量*/
void PnPsolver::qr_solve(CvMat * A, CvMat * b, CvMat * X)

更新Beta的值

void PnPsolver::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,double betas[4])

计算控制点的相机坐标

已知MTM特征值为0时的特征向量，已知deta的值，可求出控制点的相机坐标。

void PnPsolver::compute_ccs(const double * betas, const double * ut)

计算路标点的相机坐标

已知控制点的相机坐标，通过线性参数获得路标点的相机坐标

void PnPsolver::compute_pcs(void)

ICP求解相机位姿

根据路标点的世界坐标和相机坐标，估计相机的位姿。

void PnPsolver::estimate_R_and_t(double R[3][3], double t[3])

EPNP计算相机位姿

（1）计算4对控制点的世界坐标。
（2）计算线性系数α。
（3）构造M矩阵。
（4）通过SVD分解，计算MTM的0特征值对应的特征向量。
（5）计算L矩阵。
（6）计算线性系数β
（7）根据β和MTM的0特征值对应的特征向量，获得控制点的相机坐标---->根据α线性组合获得路标点的相机坐标。
（8）根据路标点在世界坐标系中的坐标和相机坐标系中的坐标，使用ICP估计相机位姿。
（9）根据相机位姿，求解每个路标点的重投影误差，判断内点个数。
（10）选择内点个数最多时的位姿为最优位姿。

double PnPsolver::compute_pose(double R[3][3], double t[3])

EPNP+RANSAC迭代求解

cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)

设置EPNP每次取的点的个数

set_maximum_number_of_correspondences(mRansacMinSet);

进行迭代

迭代的条件：
（1）当前帧的迭代次数小于阈值。
（2）所有帧的迭代次数之和小于阈值。

while(mnIterations<mRansacMaxIts || nCurrentIterations<nIterations)

随机取出四组匹配点（RANSAC）

对于EPNP，可以同时利用N对点的信息，求解这N个路标点的相机坐标，这里默认为4随机取出四组3D-2D匹配点，同时在列表中删除已经取出的点。

        // Get min set of points// 随机选取4组（默认数目）最小集合for(short i = 0; i < mRansacMinSet; ++i){int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);// 将生成的这个索引映射到给定帧的特征点idint 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();} // 选取最小集

使用EPNP估计相机位姿

compute_pose(mRi, mti);

计算内点数

重投影误差小于阈值的话就是内点，否则为外点。

CheckInliers();

获得最优的位姿

选择内点数最多的那组位姿为当前帧的最优位姿。

            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));} // 更新最佳的计算结果

进一步精求位姿

以上面求出的内点构建M矩阵，再一次求出相机位姿。

if(Refine())

ORB_SLAM2 PnPSolver相关推荐

（一次性搞定）ORB_SLAM2地图保存与加载
(一次性搞定)ORB_SLAM2地图保存与加载本文记录了ORB_SLAM2中地图保存与加载的过程. 参考博客: https://blog.csdn.net/qq_34254510/article/d ...
【学习SLAM】ORB_SLAM2 双目测试(1)
针对双目相机和RGB-D相机的ORB-SLAM2建立在单目ORB-SLAM的基础上,它的核心组件,如图2所示. 图2 ORB-SLAM2由三个平行的线程组成,跟踪,局部建图和回环检测.在一次回环检测后 ...
ORB_SLAM2局部建图线程
局部建图线程入口:可执行程序在初始化三个线程的时候,在System.cc的构造函数中进入局部建图线程 mpLocalMapper = new LocalMapping(mpMap, //指定使io ...
ORB_SLAM2中Tracking线程
Tracking线程是ORB_SLAM2的主线程.在System.cc中,使用构造函数进行了初始化,开启了三个线程. 可执行程序->System构造函数(初始化三个线程)->处理输入的 ...
ORB_SLAM2中Tracking线程的三种追踪方式
1.参考关键帧追踪模式 bool Tracking::TrackReferenceKeyFrame() 对参考关键帧中的路标点进行跟踪.在Tracking线程中,每传入一帧,都会进行位姿优化. ...
ORB_SLAM2程序入口（System.cc）
程序入口 ORB_SLAM2的程序入口为src/System.cc.在CMakeList.txt中可知,ORB_SLAM2的可执行程序为: Examples/Stereo/stereo_kitti ...
Ubuntu18.04运行ORB_SLAM2
运行环境:Ubuntu18.04 预先安装的库需要预先安装一些库,如Eign,Sophus,OpenCV等.笔者在阅读<SLAM十四讲>的时候已经安装,在此不再赘述. ORB_SLAM2 ...
ORB_SLAM2代码阅读(5)——Bundle Adjustment
ORB_SLAM2代码阅读(5)--Bundle Adjustment 1. 说明 2. Bundle Adjustment(BA)的物理意义 3. BA的数学表达 4. BA的求解方法 4.1 最速 ...
ORB_SLAM2代码阅读(3)——LocalMapping线程
ORB_SLAM2代码阅读(3)--LocalMapping线程 1.说明 2.简介 3.处理关键帧 4. 地图点剔除 5. 创建新的地图点 6.相邻搜索 6.剔除冗余关键帧 1.说明本文介绍ORB ...

ORB_SLAM2 PnPSolver