??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的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。