ORB_SLAM2 PnPSolver
??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的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 喝酒歌词是哪首歌啊？
- 下一篇： 一房一厅多少钱啊？