單目初始化

1.前提條件（640*480）

• 參與初始化的兩幀各自的特征點(diǎn)數(shù)目都需要大于100.
• 兩幀特征點(diǎn)成功匹配的數(shù)目需要大于或等于100.
• 兩幀特征點(diǎn)三角化成功的三維點(diǎn)數(shù)目需要大于50.

2.針對條件三 流程如下

1. 記錄當(dāng)前幀和參考幀（第一幀）之間的特征匹配關(guān)系。
2. 在特征匹配點(diǎn)對中隨機(jī)選擇8對匹配點(diǎn)作為一組。
3. 對這8對點(diǎn)分別計(jì)算基礎(chǔ)矩陣F和單應(yīng)矩陣H，并得到得分。
4. 計(jì)算得分比例，根據(jù)判斷選擇基礎(chǔ)矩陣還是單應(yīng)矩陣求位姿和三角化。

3. 求單應(yīng)矩陣--平面

1. 對特征匹配點(diǎn)坐標(biāo)進(jìn)行歸一化。
2. 開始迭代，每次選擇8對點(diǎn)計(jì)算單應(yīng)矩陣。
3. 根據(jù)當(dāng)次計(jì)算的單應(yīng)矩陣的重投影誤差對結(jié)果進(jìn)行評分。
4. 重復(fù)2、3步，以得分最高的單應(yīng)矩陣作為最優(yōu)的單應(yīng)矩陣。

3.1坐標(biāo)歸一化?

目的：防止坐標(biāo)值量級差別較大。

好處：能夠提高運(yùn)算結(jié)果的精度；利用歸一化處理后的圖像坐標(biāo)，對尺度縮放和原點(diǎn)的選擇不敏感。

歸一化操作步驟如下：

1.計(jì)算每個(gè)特征點(diǎn)坐標(biāo)分別在兩個(gè)坐標(biāo)軸方向上的均值meanX，meanY

2.求出平均到每個(gè)點(diǎn)上，其坐標(biāo)偏離均值meanX、meanY的程度，記為meanDevX，meanDevY，并將其倒數(shù)作為一個(gè)尺度縮放因子sX，sY。

3.用均值和尺度縮放因子對坐標(biāo)進(jìn)行歸一化

4.用矩陣表示上述變換關(guān)系，得到歸一化矩陣T

歸一化代碼如下：

 *  為什么要?dú)w一化？*  在相似變換之后(點(diǎn)在不同的坐標(biāo)系下),他們的單應(yīng)性矩陣是不相同的*  如果圖像存在噪聲,使得點(diǎn)的坐標(biāo)發(fā)生了變化,那么它的單應(yīng)性矩陣也會(huì)發(fā)生變化*  我們采取的方法是將點(diǎn)的坐標(biāo)放到同一坐標(biāo)系下,并將縮放尺度也進(jìn)行統(tǒng)一 *  對同一幅圖像的坐標(biāo)進(jìn)行相同的變換,不同圖像進(jìn)行不同變換*  縮放尺度是為了讓噪聲對于圖像的影響在一個(gè)數(shù)量級上* *  Step 1 計(jì)算特征點(diǎn)X,Y坐標(biāo)的均值 *  Step 2 計(jì)算特征點(diǎn)X,Y坐標(biāo)離均值的平均偏離程度*  Step 3 將x坐標(biāo)和y坐標(biāo)分別進(jìn)行尺度歸一化，使得x坐標(biāo)和y坐標(biāo)的一階絕對矩分別為1 *  Step 4 計(jì)算歸一化矩陣：其實(shí)就是前面做的操作用矩陣變換來表示而已* * @param[in] vKeys                               待歸一化的特征點(diǎn)* @param[in & out] vNormalizedPoints             特征點(diǎn)歸一化后的坐標(biāo)* @param[in & out] T                             歸一化特征點(diǎn)的變換矩陣*/
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)                           //將特征點(diǎn)歸一化的矩陣
{// 歸一化的是這些點(diǎn)在x方向和在y方向上的一階絕對矩（隨機(jī)變量的期望）。// Step 1 計(jì)算特征點(diǎn)X,Y坐標(biāo)的均值 meanX, meanYfloat meanX = 0;float meanY = 0;//獲取特征點(diǎn)的數(shù)量const int N = vKeys.size();//設(shè)置用來存儲(chǔ)歸一后特征點(diǎn)的向量大小，和歸一化前保持一致vNormalizedPoints.resize(N);//開始遍歷所有的特征點(diǎn)for(int i=0; i<N; i++){//分別累加特征點(diǎn)的X、Y坐標(biāo)meanX += vKeys[i].pt.x;meanY += vKeys[i].pt.y;}//計(jì)算X、Y坐標(biāo)的均值meanX = meanX/N;meanY = meanY/N;// Step 2 計(jì)算特征點(diǎn)X,Y坐標(biāo)離均值的平均偏離程度 meanDevX, meanDevY，注意不是標(biāo)準(zhǔn)差float meanDevX = 0;float meanDevY = 0;// 將原始特征點(diǎn)減去均值坐標(biāo)，使x坐標(biāo)和y坐標(biāo)均值分別為0for(int i=0; i<N; i++){vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;//累計(jì)這些特征點(diǎn)偏離橫縱坐標(biāo)均值的程度meanDevX += fabs(vNormalizedPoints[i].x);meanDevY += fabs(vNormalizedPoints[i].y);}// 求出平均到每個(gè)點(diǎn)上，其坐標(biāo)偏離橫縱坐標(biāo)均值的程度；將其倒數(shù)作為一個(gè)尺度縮放因子meanDevX = meanDevX/N;meanDevY = meanDevY/N;float sX = 1.0/meanDevX;float sY = 1.0/meanDevY;// Step 3 將x坐標(biāo)和y坐標(biāo)分別進(jìn)行尺度歸一化，使得x坐標(biāo)和y坐標(biāo)的一階絕對矩分別為1 // 這里所謂的一階絕對矩其實(shí)就是隨機(jī)變量到取值的中心的絕對值的平均值（期望）for(int i=0; i<N; i++){//對，就是簡單地對特征點(diǎn)的坐標(biāo)進(jìn)行進(jìn)一步的縮放vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;}// Step 4 計(jì)算歸一化矩陣：其實(shí)就是前面做的操作用矩陣變換來表示而已// |sX  0  -meanx*sX|// |0   sY -meany*sY|// |0   0      1    |T = cv::Mat::eye(3,3,CV_32F);T.at<float>(0,0) = sX;T.at<float>(1,1) = sY;T.at<float>(0,2) = -meanX*sX;T.at<float>(1,2) = -meanY*sY;
}

3.2求解單應(yīng)矩陣

單應(yīng)矩陣H的自由度是8，一對特征點(diǎn)可以提供兩個(gè)約束方程，所以只需要4對點(diǎn)提供8個(gè)約束方程就可以求解了。

為什么自由度是8？因?yàn)锳h=0，矩陣H共有9個(gè)元素，去掉一個(gè)自由度，就是8個(gè)自由度。

?對矩陣A進(jìn)行SVD分解

?代碼：

/*** @brief 用DLT方法求解單應(yīng)矩陣H* 這里最少用4對點(diǎn)就能夠求出來，不過這里為了統(tǒng)一還是使用了8對點(diǎn)求最小二乘解* * @param[in] vP1               參考幀中歸一化后的特征點(diǎn)* @param[in] vP2               當(dāng)前幀中歸一化后的特征點(diǎn)* @return cv::Mat              計(jì)算的單應(yīng)矩陣H*/
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, //歸一化后的點(diǎn), in reference frameconst vector<cv::Point2f> &vP2) //歸一化后的點(diǎn), in current frame
{// 基本原理：見附件推導(dǎo)過程：// |x'|     | h1 h2 h3 ||x|// |y'| = a | h4 h5 h6 ||y|  簡寫: x' = a H x, a為一個(gè)尺度因子// |1 |     | h7 h8 h9 ||1|// 使用DLT(direct linear tranform)求解該模型// x' = a H x // ---> (x') 叉乘 (H x)  = 0  (因?yàn)榉较蛳嗤? (取前兩行就可以推導(dǎo)出下面的了)// ---> Ah = 0 // A = | 0  0  0 -x -y -1 xy' yy' y'|  h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 |//     |-x -y -1  0  0  0 xx' yx' x'|// 通過SVD求解Ah = 0，A^T*A最小特征值對應(yīng)的特征向量即為解// 其實(shí)也就是右奇異值矩陣的最后一列//獲取參與計(jì)算的特征點(diǎn)的數(shù)目const int N = vP1.size();// 構(gòu)造用于計(jì)算的矩陣 A cv::Mat A(2*N,				//行，注意每一個(gè)點(diǎn)的數(shù)據(jù)對應(yīng)兩行9,				//列CV_32F);      	//float數(shù)據(jù)類型// 構(gòu)造矩陣A，將每個(gè)特征點(diǎn)添加到矩陣A中的元素for(int i=0; i<N; i++){//獲取特征點(diǎn)對的像素坐標(biāo)const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;//生成這個(gè)點(diǎn)的第一行A.at<float>(2*i,0) = 0.0;A.at<float>(2*i,1) = 0.0;A.at<float>(2*i,2) = 0.0;A.at<float>(2*i,3) = -u1;A.at<float>(2*i,4) = -v1;A.at<float>(2*i,5) = -1;A.at<float>(2*i,6) = v2*u1;A.at<float>(2*i,7) = v2*v1;A.at<float>(2*i,8) = v2;//生成這個(gè)點(diǎn)的第二行A.at<float>(2*i+1,0) = u1;A.at<float>(2*i+1,1) = v1;A.at<float>(2*i+1,2) = 1;A.at<float>(2*i+1,3) = 0.0;A.at<float>(2*i+1,4) = 0.0;A.at<float>(2*i+1,5) = 0.0;A.at<float>(2*i+1,6) = -u2*u1;A.at<float>(2*i+1,7) = -u2*v1;A.at<float>(2*i+1,8) = -u2;}// 定義輸出變量，u是左邊的正交矩陣U， w為奇異矩陣，vt中的t表示是右正交矩陣V的轉(zhuǎn)置cv::Mat u,w,vt;//使用opencv提供的進(jìn)行奇異值分解的函數(shù)cv::SVDecomp(A,							//輸入，待進(jìn)行奇異值分解的矩陣w,							//輸出，奇異值矩陣u,							//輸出，矩陣Uvt,						//輸出，矩陣V^Tcv::SVD::MODIFY_A | 		//輸入，MODIFY_A是指允許計(jì)算函數(shù)可以修改待分解的矩陣，官方文檔上說這樣可以加快計(jì)算速度、節(jié)省內(nèi)存cv::SVD::FULL_UV);		//FULL_UV=把U和VT補(bǔ)充成單位正交方陣// 返回最小奇異值所對應(yīng)的右奇異向量// 注意前面說的是右奇異值矩陣的最后一列，但是在這里因?yàn)槭莢t，轉(zhuǎn)置后了，所以是行；由于A有9列數(shù)據(jù)，故最后一列的下標(biāo)為8return vt.row(8).reshape(0, 			//轉(zhuǎn)換后的通道數(shù)，這里設(shè)置為0表示是與前面相同3); 			//轉(zhuǎn)換后的行數(shù),對應(yīng)V的最后一列
}

3.3檢驗(yàn)單應(yīng)矩陣并評分

通過單應(yīng)矩陣對已經(jīng)判斷為內(nèi)點(diǎn)的特征點(diǎn)進(jìn)行雙向投影，計(jì)算加權(quán)重投影誤差，最終選擇誤差最小的單應(yīng)矩陣作為最優(yōu)解。

特征點(diǎn)對之間雙向投影，累計(jì)所有特征點(diǎn)對中的內(nèi)點(diǎn)誤差，誤差越大評分越低。

/*** @brief 對給定的homography matrix打分,需要使用到卡方檢驗(yàn)的知識* * @param[in] H21                       從參考幀到當(dāng)前幀的單應(yīng)矩陣* @param[in] H12                       從當(dāng)前幀到參考幀的單應(yīng)矩陣* @param[in] vbMatchesInliers          匹配好的特征點(diǎn)對的Inliers標(biāo)記* @param[in] sigma                     方差，默認(rèn)為1* @return float                        返回得分*/
float Initializer::CheckHomography(const cv::Mat &H21,                 //從參考幀到當(dāng)前幀的單應(yīng)矩陣const cv::Mat &H12,                 //從當(dāng)前幀到參考幀的單應(yīng)矩陣vector<bool> &vbMatchesInliers,     //匹配好的特征點(diǎn)對的Inliers標(biāo)記float sigma)                        //估計(jì)誤差
{// 說明：在已值n維觀測數(shù)據(jù)誤差服從N(0，sigma）的高斯分布時(shí)// 其誤差加權(quán)最小二乘結(jié)果為  sum_error = SUM(e(i)^T * Q^(-1) * e(i))// 其中：e(i) = [e_x,e_y,...]^T, Q維觀測數(shù)據(jù)協(xié)方差矩陣，即sigma * sigma組成的協(xié)方差矩陣// 誤差加權(quán)最小二次結(jié)果越小，說明觀測數(shù)據(jù)精度越高// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分?jǐn)?shù)就越高// 算法目標(biāo)： 檢查單應(yīng)變換矩陣// 檢查方式：通過H矩陣，進(jìn)行參考幀和當(dāng)前幀之間的雙向投影，并計(jì)算起加權(quán)最小二乘投影誤差// 算法流程// input: 單應(yīng)性矩陣 H21, H12, 匹配點(diǎn)集 mvKeys1//    do://        for p1(i), p2(i) in mvKeys://           error_i1 = ||p2(i) - H21 * p1(i)||2//           error_i2 = ||p1(i) - H12 * p2(i)||2//           //           w1 = 1 / sigma / sigma//           w2 = 1 / sigma / sigma// //           if error1 < th//              score +=   th - error_i1 * w1//           if error2 < th//              score +=   th - error_i2 * w2// //           if error_1i > th or error_2i > th//              p1(i), p2(i) are inner points//              vbMatchesInliers(i) = true//           else //              p1(i), p2(i) are outliers//              vbMatchesInliers(i) = false//           end//        end//   output: score, inliers// 特點(diǎn)匹配個(gè)數(shù)const int N = mvMatches12.size();// Step 1 獲取從參考幀到當(dāng)前幀的單應(yīng)矩陣的各個(gè)元素const float h11 = H21.at<float>(0,0);const float h12 = H21.at<float>(0,1);const float h13 = H21.at<float>(0,2);const float h21 = H21.at<float>(1,0);const float h22 = H21.at<float>(1,1);const float h23 = H21.at<float>(1,2);const float h31 = H21.at<float>(2,0);const float h32 = H21.at<float>(2,1);const float h33 = H21.at<float>(2,2);// 獲取從當(dāng)前幀到參考幀的單應(yīng)矩陣的各個(gè)元素const float h11inv = H12.at<float>(0,0);const float h12inv = H12.at<float>(0,1);const float h13inv = H12.at<float>(0,2);const float h21inv = H12.at<float>(1,0);const float h22inv = H12.at<float>(1,1);const float h23inv = H12.at<float>(1,2);const float h31inv = H12.at<float>(2,0);const float h32inv = H12.at<float>(2,1);const float h33inv = H12.at<float>(2,2);// 給特征點(diǎn)對的Inliers標(biāo)記預(yù)分配空間vbMatchesInliers.resize(N);// 初始化score值float score = 0;// 基于卡方檢驗(yàn)計(jì)算出的閾值（假設(shè)測量有一個(gè)像素的偏差）// 自由度為2的卡方分布，顯著性水平為0.05，對應(yīng)的臨界閾值const float th = 5.991;//信息矩陣，方差平方的倒數(shù)const float invSigmaSquare = 1.0/(sigma * sigma);// Step 2 通過H矩陣，進(jìn)行參考幀和當(dāng)前幀之間的雙向投影，并計(jì)算起加權(quán)重投影誤差// H21 表示從img1 到 img2的變換矩陣// H12 表示從img2 到 img1的變換矩陣 for(int i = 0; i < N; i++){// 一開始都默認(rèn)為Inlierbool bIn = true;// Step 2.1 提取參考幀和當(dāng)前幀之間的特征匹配點(diǎn)對const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Step 2.2 計(jì)算 img2 到 img1 的重投影誤差// x1 = H12*x2// 將圖像2中的特征點(diǎn)通過單應(yīng)變換投影到圖像1中// |u1|   |h11inv h12inv h13inv||u2|   |u2in1|// |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv// |1 |   |h31inv h32inv h33inv||1 |   |  1  |// 計(jì)算投影歸一化坐標(biāo)const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;// 計(jì)算重投影誤差 = ||p1(i) - H12 * p2(i)||2const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 = squareDist1 * invSigmaSquare;// Step 2.3 用閾值標(biāo)記離群點(diǎn)，內(nèi)點(diǎn)的話累加得分if(chiSquare1>th)bIn = false;    else// 誤差越大，得分越低score += th - chiSquare1;// 計(jì)算從img1 到 img2 的投影變換誤差// x1in2 = H21*x1// 將圖像2中的特征點(diǎn)通過單應(yīng)變換投影到圖像1中// |u2|   |h11 h12 h13||u1|   |u1in2|// |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv// |1 |   |h31 h32 h33||1 |   |  1  |// 計(jì)算投影歸一化坐標(biāo)const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;// 計(jì)算重投影誤差 const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);const float chiSquare2 = squareDist2*invSigmaSquare;// 用閾值標(biāo)記離群點(diǎn)，內(nèi)點(diǎn)的話累加得分if(chiSquare2>th)bIn = false;elsescore += th - chiSquare2;   // Step 2.4 如果從img2 到 img1 和 從img1 到img2的重投影誤差均滿足要求，則說明是Inlier pointif(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}return score;
}

4.求基礎(chǔ)矩陣F--非平面

1. 對特征匹配點(diǎn)坐標(biāo)進(jìn)行歸一化
2. 選擇8個(gè)點(diǎn)對來計(jì)算基礎(chǔ)矩陣。
3. 根據(jù)當(dāng)次計(jì)算的基礎(chǔ)矩陣的重投影誤差對結(jié)果進(jìn)行評分。
4. 重復(fù)2、3步，以得分最高的基礎(chǔ)矩陣作為最優(yōu)的基礎(chǔ)矩陣。

4.1使用八點(diǎn)法求解基礎(chǔ)矩陣?

?基礎(chǔ)矩陣共有9個(gè)元素，其中尺度等價(jià)性去掉1個(gè)自由度，基礎(chǔ)矩陣秩為2，再去掉1個(gè)自由度，所以自由度應(yīng)該為7.?

SVD求解基礎(chǔ)矩陣

/*** @brief 根據(jù)特征點(diǎn)匹配求fundamental matrix（normalized 8點(diǎn)法）* 注意F矩陣有秩為2的約束，所以需要兩次SVD分解* * @param[in] vP1           參考幀中歸一化后的特征點(diǎn)* @param[in] vP2           當(dāng)前幀中歸一化后的特征點(diǎn)* @return cv::Mat          最后計(jì)算得到的基礎(chǔ)矩陣F*/
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //歸一化后的點(diǎn), in reference frameconst vector<cv::Point2f> &vP2) //歸一化后的點(diǎn), in current frame
{// 原理詳見附件推導(dǎo)// x'Fx = 0 整理可得：Af = 0// A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |// 通過SVD求解Af = 0，A'A最小特征值對應(yīng)的特征向量即為解//獲取參與計(jì)算的特征點(diǎn)對數(shù)const int N = vP1.size();//初始化A矩陣cv::Mat A(N,9,CV_32F); // N*9維// 構(gòu)造矩陣A，將每個(gè)特征點(diǎn)添加到矩陣A中的元素for(int i=0; i<N; i++){const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;A.at<float>(i,0) = u2*u1;A.at<float>(i,1) = u2*v1;A.at<float>(i,2) = u2;A.at<float>(i,3) = v2*u1;A.at<float>(i,4) = v2*v1;A.at<float>(i,5) = v2;A.at<float>(i,6) = u1;A.at<float>(i,7) = v1;A.at<float>(i,8) = 1;}//存儲(chǔ)奇異值分解結(jié)果的變量cv::Mat u,w,vt;// 定義輸出變量，u是左邊的正交矩陣U， w為奇異矩陣，vt中的t表示是右正交矩陣V的轉(zhuǎn)置cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 轉(zhuǎn)換成基礎(chǔ)矩陣的形式cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列//基礎(chǔ)矩陣的秩為2,而我們不敢保證計(jì)算得到的這個(gè)結(jié)果的秩為2,所以需要通過第二次奇異值分解,來強(qiáng)制使其秩為2// 對初步得來的基礎(chǔ)矩陣進(jìn)行第2次奇異值分解cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 秩2約束，強(qiáng)制將第3個(gè)奇異值設(shè)置為0w.at<float>(2)=0; // 重新組合好滿足秩約束的基礎(chǔ)矩陣，作為最終計(jì)算結(jié)果返回 return  u*cv::Mat::diag(w)*vt;
}

4.2驗(yàn)證基礎(chǔ)矩陣并評分

計(jì)算點(diǎn)到極線的距離，并求和，距離越大，誤差越大。p2到l2和p1到l1.

代碼：

/*** @brief 對給定的Fundamental matrix打分* * @param[in] F21                       當(dāng)前幀和參考幀之間的基礎(chǔ)矩陣* @param[in] vbMatchesInliers          匹配的特征點(diǎn)對屬于inliers的標(biāo)記* @param[in] sigma                     方差，默認(rèn)為1* @return float                        返回得分*/
float Initializer::CheckFundamental(const cv::Mat &F21,             //當(dāng)前幀和參考幀之間的基礎(chǔ)矩陣vector<bool> &vbMatchesInliers, //匹配的特征點(diǎn)對屬于inliers的標(biāo)記float sigma)                    //方差
{// 說明：在已值n維觀測數(shù)據(jù)誤差服從N(0，sigma）的高斯分布時(shí)// 其誤差加權(quán)最小二乘結(jié)果為  sum_error = SUM(e(i)^T * Q^(-1) * e(i))// 其中：e(i) = [e_x,e_y,...]^T, Q維觀測數(shù)據(jù)協(xié)方差矩陣，即sigma * sigma組成的協(xié)方差矩陣// 誤差加權(quán)最小二次結(jié)果越小，說明觀測數(shù)據(jù)精度越高// 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分?jǐn)?shù)就越高// 算法目標(biāo)：檢查基礎(chǔ)矩陣// 檢查方式：利用對極幾何原理 p2^T * F * p1 = 0// 假設(shè)：三維空間中的點(diǎn) P 在 img1 和 img2 兩圖像上的投影分別為 p1 和 p2（兩個(gè)為同名點(diǎn)）//   則：p2 一定存在于極線 l2 上，即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T//      所以，這里的誤差項(xiàng) e 為 p2 到 極線 l2 的距離，如果在直線上，則 e = 0//      根據(jù)點(diǎn)到直線的距離公式：d = (ax + by + c) / sqrt(a * a + b * b)//      所以，e =  (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)// 算法流程// input: 基礎(chǔ)矩陣 F 左右視圖匹配點(diǎn)集 mvKeys1//    do://        for p1(i), p2(i) in mvKeys://           l2 = F * p1(i)//           l1 = p2(i) * F//           error_i1 = dist_point_to_line(x2,l2)//           error_i2 = dist_point_to_line(x1,l1)//           //           w1 = 1 / sigma / sigma//           w2 = 1 / sigma / sigma// //           if error1 < th//              score +=   thScore - error_i1 * w1//           if error2 < th//              score +=   thScore - error_i2 * w2// //           if error_1i > th or error_2i > th//              p1(i), p2(i) are inner points//              vbMatchesInliers(i) = true//           else //              p1(i), p2(i) are outliers//              vbMatchesInliers(i) = false//           end//        end//   output: score, inliers// 獲取匹配的特征點(diǎn)對的總對數(shù)const int N = mvMatches12.size();// Step 1 提取基礎(chǔ)矩陣中的元素?cái)?shù)據(jù)const float f11 = F21.at<float>(0,0);const float f12 = F21.at<float>(0,1);const float f13 = F21.at<float>(0,2);const float f21 = F21.at<float>(1,0);const float f22 = F21.at<float>(1,1);const float f23 = F21.at<float>(1,2);const float f31 = F21.at<float>(2,0);const float f32 = F21.at<float>(2,1);const float f33 = F21.at<float>(2,2);// 預(yù)分配空間vbMatchesInliers.resize(N);// 設(shè)置評分初始值（因?yàn)楹竺嫘枰M(jìn)行這個(gè)數(shù)值的累計(jì)）float score = 0;// 基于卡方檢驗(yàn)計(jì)算出的閾值// 自由度為1的卡方分布，顯著性水平為0.05，對應(yīng)的臨界閾值// ?是因?yàn)辄c(diǎn)到直線距離是一個(gè)自由度嗎？const float th = 3.841;// 自由度為2的卡方分布，顯著性水平為0.05，對應(yīng)的臨界閾值const float thScore = 5.991;// 信息矩陣，或 協(xié)方差矩陣的逆矩陣const float invSigmaSquare = 1.0/(sigma*sigma);// Step 2 計(jì)算img1 和 img2 在估計(jì) F 時(shí)的score值for(int i=0; i<N; i++){//默認(rèn)為這對特征點(diǎn)是Inliersbool bIn = true;// Step 2.1 提取參考幀和當(dāng)前幀之間的特征匹配點(diǎn)對const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];// 提取出特征點(diǎn)的坐標(biāo)const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;// Reprojection error in second image// Step 2.2 計(jì)算 img1 上的點(diǎn)在 img2 上投影得到的極線 l2 = F21 * p1 = (a2,b2,c2)const float a2 = f11*u1+f12*v1+f13;const float b2 = f21*u1+f22*v1+f23;const float c2 = f31*u1+f32*v1+f33;// Step 2.3 計(jì)算誤差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num2 = a2*u2+b2*v2+c2;const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 帶權(quán)重誤差const float chiSquare1 = squareDist1*invSigmaSquare;// Step 2.4 誤差大于閾值就說明這個(gè)點(diǎn)是Outlier // ? 為什么判斷閾值用的 th（1自由度），計(jì)算得分用的thScore（2自由度）// ? 可能是為了和CheckHomography 得分統(tǒng)一？if(chiSquare1>th)bIn = false;else// 誤差越大，得分越低score += thScore - chiSquare1;// 計(jì)算img2上的點(diǎn)在 img1 上投影得到的極線 l1= p2 * F21 = (a1,b1,c1)const float a1 = f11*u2+f21*v2+f31;const float b1 = f12*u2+f22*v2+f32;const float c1 = f13*u2+f23*v2+f33;// 計(jì)算誤差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)const float num1 = a1*u1+b1*v1+c1;const float squareDist2 = num1*num1/(a1*a1+b1*b1);// 帶權(quán)重誤差const float chiSquare2 = squareDist2*invSigmaSquare;// 誤差大于閾值就說明這個(gè)點(diǎn)是Outlier if(chiSquare2>th)bIn = false;elsescore += thScore - chiSquare2;// Step 2.5 保存結(jié)果if(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;}//  返回評分return score;
}

5.特征點(diǎn)對三角化?

?

?

?

代碼：

/** 給定投影矩陣P1,P2和圖像上的匹配特征點(diǎn)點(diǎn)kp1,kp2，從而計(jì)算三維點(diǎn)坐標(biāo)* @brief * * @param[in] kp1               特征點(diǎn), in reference frame* @param[in] kp2               特征點(diǎn), in current frame* @param[in] P1                投影矩陣P1* @param[in] P2                投影矩陣P2* @param[in & out] x3D         計(jì)算的三維點(diǎn)*/
void Initializer::Triangulate(const cv::KeyPoint &kp1,    //特征點(diǎn), in reference frameconst cv::KeyPoint &kp2,    //特征點(diǎn), in current frameconst cv::Mat &P1,          //投影矩陣P1const cv::Mat &P2,          //投影矩陣P2cv::Mat &x3D)               //三維點(diǎn)
{// 原理// Trianularization: 已知匹配特征點(diǎn)對{x x'} 和 各自相機(jī)矩陣{P P'}, 估計(jì)三維點(diǎn) X// x' = P'X  x = PX// 它們都屬于 x = aPX模型//                         |X|// |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|// |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|// |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|// 采用DLT的方法：x叉乘PX = 0// |yp2 -  p1|     |0|// |p0 -  xp2| X = |0|// |xp1 - yp0|     |0|// 兩個(gè)點(diǎn):// |yp2   -  p1  |     |0|// |p0    -  xp2 | X = |0| ===> AX = 0// |y'p2' -  p1' |     |0|// |p0'   - x'p2'|     |0|// 變成程序中的形式：// |xp2  - p0 |     |0|// |yp2  - p1 | X = |0| ===> AX = 0// |x'p2'- p0'|     |0|// |y'p2'- p1'|     |0|// 然后就組成了一個(gè)四元一次正定方程組，SVD求解，右奇異矩陣的最后一行就是最終的解.//這個(gè)就是上面注釋中的矩陣Acv::Mat A(4,4,CV_32F);//構(gòu)造參數(shù)矩陣AA.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);//奇異值分解的結(jié)果cv::Mat u,w,vt;//對系數(shù)矩陣A進(jìn)行奇異值分解cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);//根據(jù)前面的結(jié)論，奇異值分解右矩陣的最后一行其實(shí)就是解，原理類似于前面的求最小二乘解，四個(gè)未知數(shù)四個(gè)方程正好正定//別忘了我們更習(xí)慣用列向量來表示一個(gè)點(diǎn)的空間坐標(biāo)x3D = vt.row(3).t();//為了符合其次坐標(biāo)的形式，使最后一維為1x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

6.檢驗(yàn)三角化結(jié)果

?確定一個(gè)合格的三維點(diǎn)需要通過以下關(guān)卡

1. 三維點(diǎn)的3個(gè)坐標(biāo)必須是有限的實(shí)數(shù)
2. 三維點(diǎn)深度值必須為正
3. 三維點(diǎn)和兩幀圖像光軸中心夾角需要滿足一定的條件。夾角越大，視差越大，三角化結(jié)果越準(zhǔn)確。
4. 三維點(diǎn)的重投影誤差小于設(shè)定的閾值。

我們會(huì)記錄當(dāng)前位姿對應(yīng)的合格的三維點(diǎn)數(shù)目和視差。位姿可能有多組解，根據(jù)如下條件選擇最佳的解。

1. 最優(yōu)解成功三角化點(diǎn)數(shù)目明顯大于次優(yōu)解的點(diǎn)數(shù)目。
2. 最優(yōu)解的視差大于設(shè)定點(diǎn)閾值。
3. 最優(yōu)解成功三角化點(diǎn)數(shù)目大于設(shè)定的閾值。
4. 最優(yōu)解成功三角化點(diǎn)數(shù)目占所有特征點(diǎn)數(shù)目的90%以上。?

代碼：?

/*** @brief 用位姿來對特征匹配點(diǎn)三角化，從中篩選中合格的三維點(diǎn)* * @param[in] R                                     旋轉(zhuǎn)矩陣R* @param[in] t                                     平移矩陣t* @param[in] vKeys1                                參考幀特征點(diǎn)  * @param[in] vKeys2                                當(dāng)前幀特征點(diǎn)* @param[in] vMatches12                            兩幀特征點(diǎn)的匹配關(guān)系* @param[in] vbMatchesInliers                      特征點(diǎn)對內(nèi)點(diǎn)標(biāo)記* @param[in] K                                     相機(jī)內(nèi)參矩陣* @param[in & out] vP3D                            三角化測量之后的特征點(diǎn)的空間坐標(biāo)* @param[in] th2                                   重投影誤差的閾值* @param[in & out] vbGood                          標(biāo)記成功三角化點(diǎn)？* @param[in & out] parallax                        計(jì)算出來的比較大的視差角（注意不是最大，具體看后面代碼）* @return int */
int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax)
{   // 對給出的特征點(diǎn)對及其R t , 通過三角化檢查解的有效性，也稱為 cheirality check// Calibration parameters//從相機(jī)內(nèi)參數(shù)矩陣獲取相機(jī)的校正參數(shù)const float fx = K.at<float>(0,0);const float fy = K.at<float>(1,1);const float cx = K.at<float>(0,2);const float cy = K.at<float>(1,2);//特征點(diǎn)是否是good點(diǎn)的標(biāo)記，這里的特征點(diǎn)指的是參考幀中的特征點(diǎn)vbGood = vector<bool>(vKeys1.size(),false);//重設(shè)存儲(chǔ)空間坐標(biāo)的點(diǎn)的大小vP3D.resize(vKeys1.size());//存儲(chǔ)計(jì)算出來的每對特征點(diǎn)的視差vector<float> vCosParallax;vCosParallax.reserve(vKeys1.size());// Camera 1 Projection Matrix K[I|0]// Step 1：計(jì)算相機(jī)的投影矩陣  // 投影矩陣P是一個(gè) 3x4 的矩陣，可以將空間中的一個(gè)點(diǎn)投影到平面上，獲得其平面坐標(biāo)，這里均指的是齊次坐標(biāo)。// 對于第一個(gè)相機(jī)是 P1=K*[I|0]// 以第一個(gè)相機(jī)的光心作為世界坐標(biāo)系, 定義相機(jī)的投影矩陣cv::Mat P1(3,4,				//矩陣的大小是3x4CV_32F,			//數(shù)據(jù)類型是浮點(diǎn)數(shù)cv::Scalar(0));	//初始的數(shù)值是0//將整個(gè)K矩陣拷貝到P1矩陣的左側(cè)3x3矩陣，因?yàn)?K*I = KK.copyTo(P1.rowRange(0,3).colRange(0,3));// 第一個(gè)相機(jī)的光心設(shè)置為世界坐標(biāo)系下的原點(diǎn)cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);// Camera 2 Projection Matrix K[R|t]// 計(jì)算第二個(gè)相機(jī)的投影矩陣 P2=K*[R|t]cv::Mat P2(3,4,CV_32F);R.copyTo(P2.rowRange(0,3).colRange(0,3));t.copyTo(P2.rowRange(0,3).col(3));//最終結(jié)果是K*[R|t]P2 = K*P2;// 第二個(gè)相機(jī)的光心在世界坐標(biāo)系下的坐標(biāo)cv::Mat O2 = -R.t()*t;//在遍歷開始前，先將good點(diǎn)計(jì)數(shù)設(shè)置為0int nGood=0;// 開始遍歷所有的特征點(diǎn)對for(size_t i=0, iend=vMatches12.size();i<iend;i++){// 跳過outliersif(!vbMatchesInliers[i])continue;// Step 2 獲取特征點(diǎn)對，調(diào)用Triangulate() 函數(shù)進(jìn)行三角化，得到三角化測量之后的3D點(diǎn)坐標(biāo)// kp1和kp2是匹配好的有效特征點(diǎn)const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];//存儲(chǔ)三維點(diǎn)的的坐標(biāo)cv::Mat p3dC1;// 利用三角法恢復(fù)三維點(diǎn)p3dC1Triangulate(kp1,kp2,	//特征點(diǎn)P1,P2,		//投影矩陣p3dC1);		//輸出，三角化測量之后特征點(diǎn)的空間坐標(biāo)		// Step 3 第一關(guān)：檢查三角化的三維點(diǎn)坐標(biāo)是否合法（非無窮值）// 只要三角測量的結(jié)果中有一個(gè)是無窮大的就說明三角化失敗，跳過對當(dāng)前點(diǎn)的處理，進(jìn)行下一對特征點(diǎn)的遍歷 if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))){//其實(shí)這里就算是不這樣寫也沒問題，因?yàn)槟J(rèn)的匹配點(diǎn)對就不是good點(diǎn)vbGood[vMatches12[i].first]=false;//繼續(xù)對下一對匹配點(diǎn)的處理continue;}// Check parallax// Step 4 第二關(guān)：通過三維點(diǎn)深度值正負(fù)、兩相機(jī)光心視差角大小來檢查是否合法 //得到向量PO1cv::Mat normal1 = p3dC1 - O1;//求取模長，其實(shí)就是距離float dist1 = cv::norm(normal1);//同理構(gòu)造向量PO2cv::Mat normal2 = p3dC1 - O2;//求模長float dist2 = cv::norm(normal2);//根據(jù)公式：a.*b=|a||b|cos_theta 可以推導(dǎo)出來下面的式子float cosParallax = normal1.dot(normal2)/(dist1*dist2);// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 如果深度值為負(fù)值，為非法三維點(diǎn)跳過該匹配點(diǎn)對// ?視差比較小時(shí)，重投影誤差比較大。這里0.99998 對應(yīng)的角度為0.36°,這里不應(yīng)該是 cosParallax>0.99998 嗎？// ?因?yàn)楹竺媾袛鄓bGood 點(diǎn)時(shí)的條件也是 cosParallax<0.99998 // !可能導(dǎo)致初始化不穩(wěn)定if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)continue;// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)// 講空間點(diǎn)p3dC1變換到第2個(gè)相機(jī)坐標(biāo)系下變?yōu)閜3dC2cv::Mat p3dC2 = R*p3dC1+t;	//判斷過程和上面的相同if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)continue;// Step 5 第三關(guān)：計(jì)算空間點(diǎn)在參考幀和當(dāng)前幀上的重投影誤差，如果大于閾值則舍棄// Check reprojection error in first image// 計(jì)算3D點(diǎn)在第一個(gè)圖像上的投影誤差//投影到參考幀圖像上的點(diǎn)的坐標(biāo)x,yfloat im1x, im1y;//這個(gè)使能空間點(diǎn)的z坐標(biāo)的倒數(shù)float invZ1 = 1.0/p3dC1.at<float>(2);//投影到參考幀圖像上。因?yàn)閰⒖紟碌南鄼C(jī)坐標(biāo)系和世界坐標(biāo)系重合，因此這里就直接進(jìn)行投影就可以了im1x = fx*p3dC1.at<float>(0)*invZ1+cx;im1y = fy*p3dC1.at<float>(1)*invZ1+cy;//參考幀上的重投影誤差，這個(gè)的確就是按照定義來的float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);// 重投影誤差太大，跳過淘汰if(squareError1>th2)continue;// Check reprojection error in second image// 計(jì)算3D點(diǎn)在第二個(gè)圖像上的投影誤差，計(jì)算過程和第一個(gè)圖像類似float im2x, im2y;// 注意這里的p3dC2已經(jīng)是第二個(gè)相機(jī)坐標(biāo)系下的三維點(diǎn)了float invZ2 = 1.0/p3dC2.at<float>(2);im2x = fx*p3dC2.at<float>(0)*invZ2+cx;im2y = fy*p3dC2.at<float>(1)*invZ2+cy;// 計(jì)算重投影誤差float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);// 重投影誤差太大，跳過淘汰if(squareError2>th2)continue;// Step 6 統(tǒng)計(jì)經(jīng)過檢驗(yàn)的3D點(diǎn)個(gè)數(shù)，記錄3D點(diǎn)視差角 // 如果運(yùn)行到這里就說明當(dāng)前遍歷的這個(gè)特征點(diǎn)對靠譜，經(jīng)過了重重檢驗(yàn)，說明是一個(gè)合格的點(diǎn)，稱之為good點(diǎn) vCosParallax.push_back(cosParallax);//存儲(chǔ)這個(gè)三角化測量后的3D點(diǎn)在世界坐標(biāo)系下的坐標(biāo)vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));//good點(diǎn)計(jì)數(shù)++nGood++;//判斷視差角，只有視差角稍稍大一丟丟的才會(huì)給打good點(diǎn)標(biāo)記//? bug 我覺得這個(gè)寫的位置不太對。你的good點(diǎn)計(jì)數(shù)都++了然后才判斷，不是會(huì)讓good點(diǎn)標(biāo)志和good點(diǎn)計(jì)數(shù)不一樣嗎if(cosParallax<0.99998)vbGood[vMatches12[i].first]=true;}// Step 7 得到3D點(diǎn)中較小的視差角，并且轉(zhuǎn)換成為角度制表示if(nGood>0){// 從小到大排序，注意vCosParallax值越大，視差越小sort(vCosParallax.begin(),vCosParallax.end());// !排序后并沒有取最小的視差角，而是取一個(gè)較小的視差角// 作者的做法：如果經(jīng)過檢驗(yàn)過后的有效3D點(diǎn)小于50個(gè)，那么就取最后那個(gè)最小的視差角(cos值最大)// 如果大于50個(gè)，就取排名第50個(gè)的較小的視差角即可，為了避免3D點(diǎn)太多時(shí)出現(xiàn)太小的視差角 size_t idx = min(50,int(vCosParallax.size()-1));//將這個(gè)選中的角弧度制轉(zhuǎn)換為角度制parallax = acos(vCosParallax[idx])*180/CV_PI;}else//如果沒有g(shù)ood點(diǎn)那么這個(gè)就直接設(shè)置為0了parallax=0;//返回good點(diǎn)計(jì)數(shù)return nGood;
}