ORB-SLAM2学习笔记——epnp求解（代码好长阿）

epnp求解

1、理论部分

2、代码部分（作者牛pi）

//对pnp求解后值进行保存
PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),mnIterations(0), mnBestInliers(0), N(0)
{mvpMapPointMatches = vpMapPointMatches;mvP2D.reserve(F.mvpMapPoints.size());mvSigma2.reserve(F.mvpMapPoints.size());mvP3Dw.reserve(F.mvpMapPoints.size());mvKeyPointIndices.reserve(F.mvpMapPoints.size());mvAllIndices.reserve(F.mvpMapPoints.size());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];//对应的像素存入mvp2D中mvP2D.push_back(kp.pt);//mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);//得到与世界坐标中的位姿关系cv::Mat Pos = pMP->GetWorldPos();//导入世界坐标到mvP3D中mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0),Pos.at<float>(1), Pos.at<float>(2)));//导入关键点编号mvKeyPointIndices.push_back(i);//导入2d点的编号// mvAllIndices为所有参与PnP的2D点的索引mvAllIndices.push_back(idx);               idx++;}}}// Set camera calibration parameters//相机外参fu = F.fx;fv = F.fy;uc = F.cx;vc = F.cy;SetRansacParameters();
}
//计算完成，释放内存
PnPsolver::~PnPsolver()
{delete [] pws;delete [] us;delete [] alphas;delete [] pcs;
}//概率 最小内点数量 最大迭代次数 最小置位 系数（用于计算最小内点） 阈值
void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
{mRansacProb = probability;mRansacMinInliers = minInliers;mRansacMaxIts = maxIterations;mRansacEpsilon = epsilon;mRansacMinSet = minSet;//匹配点的数量N = mvP2D.size(); // number of correspondences//标志位（布尔类型）mvbInliersi.resize(N);// Adjust Parameters according to number of correspondences//根据相关点的数量进行调整参数//初始化最小内点数量int nMinInliers = N*mRansacEpsilon;//更新内点数if(nMinInliers<mRansacMinInliers)nMinInliers=mRansacMinInliers;//内点数过少会被重置if(nMinInliers<minSet)nMinInliers=minSet;mRansacMinInliers = nMinInliers;//更新系数 mRansacMinInliers > nMinInliers//传入的内点比我们要求的多if(mRansacEpsilon<(float)mRansacMinInliers/N)mRansacEpsilon=(float)mRansacMinInliers/N;// Set RANSAC iterations according to probability, epsilon, and max iterations//根据各个参数调整迭代次数int nIterations;//内点数等于匹配点数 迭代一次就行if(mRansacMinInliers==N)nIterations=1;else//ceil：大于或等于的最小整数 pow（x， y） x的y次方nIterations = ceil(log(1-mRansacProb)/log(1-pow(mRansacEpsilon,3)));//mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));//最大误差mvMaxError.resize(mvSigma2.size());//给允许的最大误差赋值for(size_t i=0; i<mvSigma2.size(); i++)mvMaxError[i] = mvSigma2[i]*th2;
}
//
cv::Mat PnPsolver::find(vector<bool> &vbInliers, int &nInliers)
{bool bFlag;//iterate（最大迭代次数， 标志位， 内点标志位， 内点数量）return iterate(mRansacMaxIts,bFlag,vbInliers,nInliers);    
}
//iterate（迭代次数， 标志位（是否要迭代），（内点标志位）， （内点数量） ）
cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
{//初始化bNoMore = false;vbInliers.clear();nInliers=0;//set_maximum_number_of_correspondences(mRansacMinSet);//匹配点小于内点 结束迭代if(N<mRansacMinInliers){bNoMore = true;return cv::Mat();}// mvAllIndices为所有参与PnP的2D点的索引// vAvailableIndices为每次从mvAllIndices中随机挑选mRansacMinSet组3D-2D对应点进行一次RANSACvector<size_t> vAvailableIndices;//当前迭代次数int nCurrentIterations = 0;//迭代次数小于最大迭代次数， 当前迭代次数小于要求次数 继续迭代while(mnIterations<mRansacMaxIts || nCurrentIterations<nIterations){nCurrentIterations++;mnIterations++;//重置相关性reset_correspondences();//把序号存好vAvailableIndices = mvAllIndices;// Get min set of pointsfor(short i = 0; i < mRansacMinSet; ++i){int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);int idx = vAvailableIndices[randi];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 posecompute_pose(mRi, mti);//检查内点// Check inliersCheckInliers();//当前内点数目 >= 要求的最小内点数目（根据匹配点数决定的）if(mnInliersi>=mRansacMinInliers){// If it is the best solution so far, save it//如果比最优内点数目还多if(mnInliersi>mnBestInliers){ //替换最优点//内点布尔类型mvbBestInliers = mvbInliersi;//数量mnBestInliers = mnInliersi;//定义 旋转矩阵 平移向量 还有 T 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()){nInliers = mnRefinedInliers;//所有标志位置于falsevbInliers = vector<bool>(mvpMapPointMatches.size(),false);//遍历所有点 如果为重新定义的点 标志位truefor(int i=0; i<N; i++){if(mvbRefinedInliers[i])vbInliers[mvKeyPointIndices[i]] = true;}//得到T return mRefinedTcw.clone();}}}//当前迭代次数大于要求最大迭代次数if(mnIterations>=mRansacMaxIts){   bNoMore=true;//内殿不够if(mnBestInliers>=mRansacMinInliers){//最好内点数替换当前内点数nInliers=mnBestInliers;//当前内点标识 falsevbInliers = vector<bool>(mvpMapPointMatches.size(),false);//遍历所有最好内殿 标志为truefor(int i=0; i<N; i++){if(mvbBestInliers[i])vbInliers[mvKeyPointIndices[i]] = true;}//返回 Treturn mBestTcw.clone();}}return cv::Mat();
}bool PnPsolver::Refine()
{vector<int> vIndices;vIndices.reserve(mvbBestInliers.size());for(size_t i=0; i<mvbBestInliers.size(); i++){if(mvbBestInliers[i]){vIndices.push_back(i);}}set_maximum_number_of_correspondences(vIndices.size());reset_correspondences();for(size_t i=0; i<vIndices.size(); i++){int idx = vIndices[i];add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);}// Compute camera posecompute_pose(mRi, mti);// Check inliersCheckInliers();mnRefinedInliers =mnInliersi;mvbRefinedInliers = mvbInliersi;if(mnInliersi>mRansacMinInliers){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);mRefinedTcw = cv::Mat::eye(4,4,CV_32F);Rcw.copyTo(mRefinedTcw.rowRange(0,3).colRange(0,3));tcw.copyTo(mRefinedTcw.rowRange(0,3).col(3));return true;}return false;
}//检查内点是否合格  用误差检查
void PnPsolver::CheckInliers()
{mnInliersi=0;for(int i=0; i<N; i++){cv::Point3f P3Dw = mvP3Dw[i];cv::Point2f P2D = mvP2D[i];//求相机平面坐标float Xc = mRi[0][0]*P3Dw.x+mRi[0][1]*P3Dw.y+mRi[0][2]*P3Dw.z+mti[0];float Yc = mRi[1][0]*P3Dw.x+mRi[1][1]*P3Dw.y+mRi[1][2]*P3Dw.z+mti[1];float invZc = 1/(mRi[2][0]*P3Dw.x+mRi[2][1]*P3Dw.y+mRi[2][2]*P3Dw.z+mti[2]);//求像素坐标double ue = uc + fu * Xc * invZc;double ve = vc + fv * Yc * invZc;//求误差float distX = P2D.x-ue;float distY = P2D.y-ve;float error2 = distX*distX+distY*distY;//误差小于要求最大误差if(error2<mvMaxError[i]){//当前的内点为好点mvbInliersi[i]=true;mnInliersi++;}//否则判定为坏点else{mvbInliersi[i]=false;}}
}//给一些相关系数赋值
// number_of_correspondences为RANSAC每次PnP求解时时3D点和2D点匹配对数
// RANSAC需要很多次，maximum_number_of_correspondences为匹配对数最大值
// 这个变量用于决定pws us alphas pcs容器的大小，因此只能逐渐变大不能减小void PnPsolver::set_maximum_number_of_correspondences(int n)
{//如果maximum_number_of_correspondences之前设置的过小，则重新设置，并重新初始化pws us alphas pcs的大小if (maximum_number_of_correspondences < n) {if (pws != 0) delete [] pws;if (us != 0) delete [] us;if (alphas != 0) delete [] alphas;if (pcs != 0) delete [] pcs;maximum_number_of_correspondences = n;pws = new double[3 * maximum_number_of_correspondences];us = new double[2 * maximum_number_of_correspondences];alphas = new double[4 * maximum_number_of_correspondences];pcs = new double[3 * maximum_number_of_correspondences];}
}
//重置匹配点的数目
void PnPsolver::reset_correspondences(void)
{number_of_correspondences = 0;
}
//添加匹配点对参数进行修改
void PnPsolver::add_correspondence(double X, double Y, double Z, double u, double v)
{pws[3 * number_of_correspondences    ] = X;pws[3 * number_of_correspondences + 1] = Y;pws[3 * number_of_correspondences + 2] = Z;us[2 * number_of_correspondences    ] = u;us[2 * number_of_correspondences + 1] = v;number_of_correspondences++;
}
//筛选控制点
void PnPsolver::choose_control_points(void)
{// Take C0 as the reference points centroid:// 步骤1：第一个控制点：参与PnP计算的参考3D点的几何中心cws[0][0] = cws[0][1] = cws[0][2] = 0;//求所有参考帧3D点的坐标的和for(int i = 0; i < number_of_correspondences; i++)for(int j = 0; j < 3; j++)cws[0][j] += pws[3 * i + j];//求3D点的几何中心for(int j = 0; j < 3; j++)cws[0][j] /= number_of_correspondences;// Take C1, C2, and C3 from PCA on the reference points:// 步骤2：计算其它三个控制点，C1, C2, C3通过PCA分解得到// 将所有的3D参考点写成矩阵，(number_of_correspondences *　３)的矩阵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);CvMat DC      = cvMat(3, 1, CV_64F, dc);CvMat UCt     = cvMat(3, 3, CV_64F, uct);// 步骤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];// 步骤2.2：利用SVD分解P'P可以获得P的主分量// 类似于齐次线性最小二乘求解的过程，// PW0的转置乘以PW0cvMulTransposed(PW0, &PW0tPW0, 1);//PW0的转置乘以PW0进行SVD分解cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);cvReleaseMat(&PW0);// 步骤2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量for(int i = 1; i < 4; i++) {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];}
}//得到四个控制点// 求解四个控制点的系数alphas
// (a2 a3 a4)' = inverse(cws2-cws1 cws3-cws1 cws4-cws1)*(pws-cws1)，a1 = 1-a2-a3-a4
// 每一个3D控制点，都有一组alphas与之对应
// cws1 cws2 cws3 cws4为四个控制点的坐标
// pws为3D参考点的坐标
void PnPsolver::compute_barycentric_coordinates(void)
{double cc[3 * 3], cc_inv[3 * 3];CvMat CC     = cvMat(3, 3, CV_64F, cc);CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);for(int i = 0; i < 3; i++)for(int j = 1; j < 4; j++)cc[3 * i + j - 1] = cws[j][i] - cws[0][i];cvInvert(&CC, &CC_inv, CV_SVD);double * ci = cc_inv;for(int i = 0; i < number_of_correspondences; i++) {double * pi = pws + 3 * i;double * a = alphas + 4 * i;for(int j = 0; j < 3; j++)a[1 + j] =ci[3 * j    ] * (pi[0] - cws[0][0]) +ci[3 * j + 1] * (pi[1] - cws[0][1]) +ci[3 * j + 2] * (pi[2] - cws[0][2]);a[0] = 1.0f - a[1] - a[2] - a[3];}
}