- PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):
pws(0), us(0), alphas(0), pcs(0), //這里的四個變量都是指針啊,直接這樣子寫的原因可以參考函數(shù) set_maximum_number_of_correspondences()
maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),
mnIterations(0), mnBestInliers(0), N(0)
構(gòu)造函數(shù)取出2維特征點系宫，世界坐標系下的3D點兵志，只記錄坐標，特征點在哪一層取出來的，調(diào)用SetRansacParameters()，設(shè)置默認的RANSAC參數(shù),這個和Sim3Solver中的操作是相同的 - void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
設(shè)置RANSAC迭代的參數(shù)：- 獲取給定的參數(shù)
- 計算理論內(nèi)點數(shù)
- 調(diào)整內(nèi)點數(shù)/總體數(shù)比例
- 計算理論迭代次數(shù)
- 計算不同圖層上的特征點在進行內(nèi)點檢驗時使用的不同判斷誤差閾值
- double PnPsolver::compute_pose(double R[3][3], double t[3])
根據(jù)類成員變量中給出的匹配點,計算相機位姿- 獲取EPnP算法中的四個控制點
- 計算世界坐標系下每個3D點用4個控制點線性表達時的系數(shù)alphas
- 構(gòu)造M矩陣，EPnP原始論文中公式(3)(4)-->(5)(6)(7); 矩陣的大小為 2n*12 ,n 為使用的匹配點的對數(shù)
- 求解Mx = 0,x是控制點在相機坐標系下組成的向量
-
,
是M特征向量，
需要分N=1、2孽锥、3、4計算
- 根據(jù)匹配點在相機坐標系下的坐標以及在世界坐標系下的坐標细层，然后根據(jù)ICP算法求得相機位姿惜辑。
- void PnPsolver::choose_control_points(void)
從給定的匹配點中計算出四個控制點- 第一個控制點為所有3D點幾何中心
// 遍歷每個匹配點中的3D點，然后對每個坐標軸加和
for(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;
- 其它三個控制點疫赎，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]; // 下面變量的數(shù)據(jù)區(qū)
CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0); // 如變量名所示.其實這里使用cv::Mat格式主要是為了進行SVD分解
CvMat DC = cvMat(3, 1, CV_64F, dc); // 分解上面矩陣得到的奇異值組成的矩陣
CvMat UCt = cvMat(3, 3, CV_64F, uct); // 分解上面矩陣得到的左奇異矩陣
// step 2.1：將存在pws中的參考3D點減去第一個控制點的坐標（相當于把第一個控制點作為原點）, 并存入PW0
for(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：利用SVD分解P'P可以獲得P的主分量
cvMulTransposed(PW0, &PW0tPW0, 1);
cvSVD(&PW0tPW0, // A
&DC, // W
&UCt, // U
0, // V
CV_SVD_MODIFY_A | CV_SVD_U_T); // flags
cvReleaseMat(&PW0);
// step 2.3：得到C1, C2, C3三個3D控制點，最后加上之前減掉的第一個控制點這個偏移量
// 講道理這里的條件不應寫成4,而應該是變量 number_of_correspondences 啊
for(int i = 1; i < 4; i++) {
// 這里也是只有前三個奇異值
double k = sqrt(dc[i - 1] / number_of_correspondences);
for(int j = 0; j < 3; j++)
//? 但是這里為什么要乘k捧搞？
cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
-
void PnPsolver::compute_barycentric_coordinates(void)
求解四個控制點的系數(shù)alphas
每個三維點可以通過下面式子算出和控制點之間的關(guān)系:
image
具體推導過程見 https://blog.csdn.net/qq_30356613/article/details/80588134
-
void PnPsolver::fill_M(CvMat * M,
const int row, const double * as, const double u, const double v)
填充最小二乘的M矩陣抵卫，對每一個3D參考點，Mx=0胎撇，x為四個控制點組成的向量1*12：
image
M推導過程見 https://blog.csdn.net/qq_30356613/article/details/80588134
double PnPsolver::compute_R_and_t(const double * ut, const double * betas,double R[3][3], double t[3])
根據(jù)已經(jīng)得到的控制點在當前相機坐標系下的坐標來恢復出相機的位姿
