目錄
一、地圖點初始化
二、重新記錄特征點的匹配關系
1、構建旋轉直方圖
1.1、在半徑視窗內搜索當前幀F2中所有的候選匹配特征點GetFeaturesInArea
1.2、表示一個影像像素相當于多少個影像網格列和行
1.4、遍歷圓形區域內的所有網格，尋找滿足條件的候選特征點，并將其index放到輸出里
1.5、根據閾值 和 角度投票剔除誤匹配
1.6、計算匹配點旋轉角度差所在的直方圖
1.7、根據方向剔除誤匹配的點
1.8、將最后通過篩選匹配好的特征點進行保存
三、在所有匹配特征點對中隨機選擇8對匹配特征點為一組，用于估計H矩陣和F矩陣
構造執行緒來計算H矩陣及其得分
具體步驟
1、將當前幀和參考幀的特征點坐標進行歸一化（對應函式Initializer::Normalize）
為什么要歸一化
預先對影像坐標進行歸一化有以下好處：
歸一化具體操作
2、選擇8個歸一化的點進行迭代
3、利用生成的8個歸一化特征點對, 呼叫函式 Initializer::ComputeH21() 使用八點法計算單應矩陣
4、利用重投影誤差為當次RANSAC的結果評分
5、更新具有最優評分的單應矩陣計算結果,并且保存所對應的特征點對的內點標記
計算基礎矩陣，假設場景為非平面情況下通過前兩幀求取Fundamental矩陣，得到該模型的評分
八點法計算基礎矩陣
SVD
SVD分解結果
F矩陣秩為2
使用opencv提供的進行奇異值分解的函式
四、對給定的homography matrix打分,需要使用到卡方檢驗的知識
卡方檢驗
為什么要參考卡方檢驗?
卡方分布用途
卡方分布假設檢驗步驟
決策原則
五、從基礎矩陣F中求解位姿R，t及三維點
六、用H矩陣恢復R, t和三維點
流程:
一、地圖點初始化
https://blog.csdn.net/m0_58173801/article/details/119891999?utm_source=app&app_version=4.14.1
在看代碼之前可以先看看我前面2D-2D對極幾何的介紹，里面詳細的說明了本質矩陣E和基礎矩陣F的具體求解步驟，
初始換函式（Initialize）:通過兩幀匹配關系恢復幀間運動，并計算地圖點的位置，
為什么要初始化
因為剛開始沒有地圖點和初始位姿，用兩幀匹配好的特征點三角化得到很多個三維點，用三維點做地圖來跟蹤下一幀，尺度歸一化，初始化為場景的平均深度，
先計算基礎矩陣和單應性矩陣，選取最佳的來恢復出最開始兩幀之間的相對姿態，并進行三角化得到初始地圖點，
* Step 1 重新記錄特征點對的匹配關系 * Step 2 在所有匹配特征點對中隨機選擇8對匹配特征點為一組，用于估計H矩陣和F矩陣 * Step 3 計算fundamental 矩陣 和homography 矩陣，為了加速分別開了執行緒計算 * Step 4 計算得分比例來判斷選取哪個模型來求位姿R,t
一些重要的引數
* @param[in] ReferenceFrame 參考幀 * @param[in] sigma 測量誤差 * @param[in] iterations RANSAC迭代次數 * @param[in] CurrentFrame 當前幀，也就是SLAM意義上的第二幀 * @param[in] vMatches12 當前幀（2）和參考幀（1）影像中特征點的匹配關系 * vMatches12[i]解釋：i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值 * 沒有匹配關系的話，vMatches12[i]值為 -1 * @param[in & out] R21 相機從參考幀到當前幀的旋轉 * @param[in & out] t21 相機從參考幀到當前幀的平移 * @param[in & out] vP3D 三角化測量之后的三維地圖點 * @param[in & out] vbTriangulated 標記三角化點是否有效，有效為true * @return true 該幀可以成功初始化，回傳true * @return false 該幀不滿足初始化條件，回傳false
二、重新記錄特征點的匹配關系
單目初始化中用于參考幀和當前幀的特征點匹配
* 步驟 * Step 1 構建旋轉直方圖 * Step 2 在半徑視窗內搜索當前幀F2中所有的候選匹配特征點 * Step 3 遍歷搜索搜索視窗中的所有潛在的匹配候選點，找到最優的和次優的 * Step 4 對最優次優結果進行檢查，滿足閾值、最優/次優比例，洗掉重復匹配 * Step 5 計算匹配點旋轉角度差所在的直方圖 * Step 6 篩除旋轉直方圖中“非主流”部分 * Step 7 將最后通過篩選的匹配好的特征點保存
一些重要的引數
* @param[in] F1 初始化參考幀 * @param[in] F2 當前幀 * @param[in & out] vbPrevMatched 本來存盤的是參考幀的所有特征點坐標，該函式更新為匹 配好的當前幀的特征點坐標 * @param[in & out] vnMatches12 保存參考幀F1中特征點是否匹配上，index保存是F1對應特征點索引，值保存的是匹配好的F2特征點索引 * @param[in] windowSize 搜索視窗 * @return int 回傳成功匹配的特征點數目
1、構建旋轉直方圖
HISTO_LENGTH = 30
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
// 每個bin里預分配500個，因為使用的是vector不夠的話可以自動擴展容量
rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f;
// 匹配點對距離，注意是按照F2特征點數目分配空間
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
// 從幀2到幀1的反向匹配，注意是按照F2特征點數目分配空間
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
// 遍歷幀1中的所有特征點
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
cv::KeyPoint kp1 = F1.mvKeysUn[i1];
int level1 = kp1.octave;
// 只使用原始影像上提取的特征點
if(level1>0)
continue;1.1、在半徑視窗內搜索當前幀F2中所有的候選匹配特征點GetFeaturesInArea
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
// 計算特征點x,y坐標落在哪個網格內，網格坐標為posX，posY
// mfGridElementWidthInv=(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
// mfGridElementHeightInv=(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);1.2、表示一個影像像素相當于多少個影像網格列和行
// 表示一個影像像素相當于多少個影像網格列（寬）
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 表示一個影像像素相當于多少個影像網格行（高）
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);1.3、計算半徑為r的圓左右上下邊界所在網格列和行的ID
首先我們要查找半徑為r的圓左側邊界所在網格列坐標，這個地方有點繞，慢慢理解下： (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每個網格可以平均分得幾個像素（肯定大于1）
mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒數，表示每個像素可以均分幾個網格列（肯定小于1）
(x-mnMinX-r)，可以看做是從影像的左邊界mnMinX到半徑r的圓的左邊界區域占的像素列數
兩者相乘，就是求出那個半徑為r的圓的左側邊界在哪個網格列中
保證nMinCellX 結果大于等于0
//計算半徑為r的圓左右上下邊界所在網格列和行的ID
const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv))
// 如果最終求得的圓的左邊界所在的網格列超過了設定了上限，那么就說明計算出錯，找不到符合要求的特征點，回傳空vector
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
// 計算圓所在的右邊界網格列索引
const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
// 如果計算出的圓右邊界所在的網格不合法，說明該特征點不好，直接回傳空vector
if(nMaxCellX<0)
return vIndices;
//后面的操作也都是類似的，計算出這個圓上下邊界所在的網格行的id
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
1.4、遍歷圓形區域內的所有網格，尋找滿足條件的候選特征點，并將其index放到輸出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 獲取這個網格內的所有特征點在 Frame::mvKeysUn 中的索引
const vector<size_t> vCell = mGrid[ix][iy];
// 如果這個網格中沒有特征點，那么跳過這個網格繼續下一個
if(vCell.empty())
continue;
// 如果這個網格中有特征點，那么遍歷這個影像網格中所有的特征點
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
// 根據索引先讀取這個特征點
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
// 通過檢查，計算候選特征點到圓中心的距離，查看是否是在這個圓形區域之內
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 如果x方向和y方向的距離都在指定的半徑之內，存盤其index為候選特征點
if(fabs(distx)<r && fabs(disty)<r)
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
1.5、根據閾值 和 角度投票剔除誤匹配
匹配距離必須小于設定閾值
if(bestDist1<=TH_LOW)
{
// Step 4.2：第二關篩選：最佳匹配比次佳匹配明顯要好，那么最佳匹配才真正靠譜
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
// Step 4.3：記錄成功匹配特征點的對應的地圖點(來自關鍵幀)
vpMapPointMatches[bestIdxF]=pMP;
// 這里的realIdxKF是當前遍歷到的關鍵幀的特征點id
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
1.6、計算匹配點旋轉角度差所在的直方圖
if(mbCheckOrientation)
{
// angle：每個特征點在提取描述子時的旋轉主方向角度，如果影像旋轉了，這個角度將發生改變
// 所有的特征點的角度變化應該是一致的，通過直方圖統計得到最準確的角度變化值
float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 該特征點的角度變化值
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);// 將rot分配到bin組, 四舍五入, 其實就是離散到對應的直方圖組中
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdxF); // 直方圖統計
}
nmatches++;1.7、根據方向剔除誤匹配的點
if(mbCheckOrientation)
{
// index
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 篩選出在旋轉角度差落在在直方圖區間內數量最多的前三個bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
// 如果特征點的旋轉角度變化量屬于這三個組，則保留
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配對，因為他們不符合“主流旋轉方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
return nmatches;
}
1.8、將最后通過篩選匹配好的特征點進行保存
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;三、在所有匹配特征點對中隨機選擇8對匹配特征點為一組，用于估計H矩陣和F矩陣
共選擇 mMaxIterations (默認200) 組 ，mvSets用于保存每次迭代時所使用的向量，
將上面匹配好的特征點重新記錄特征點對的匹配關系存盤在mvMatches12，是否有匹配存盤在mvbMatched1，在所有匹配特征點對中隨機選擇8對匹配特征點為一組，用于估計H矩陣和F矩陣，而且在計算fundamental 矩陣 和homography 矩陣，為了加速分別開了執行緒計算，執行緒計算也是我們前面介紹過的加鎖和釋放鎖，主要目的是為了提高計算速度，計算出來的單應矩陣和基礎矩陣的RANSAC評分，這里其實是采用重投影誤差來計算的，
構造執行緒來計算H矩陣及其得分
詳細的求解原理已經在前面細講過了，大家可以看前面連接上面的文章，這里就把最后推匯出來的公式拿出來了，
一對點提供兩個約束等式，單應矩陣H總共有9個元素，8個自由度（尺度等價性），所以需要4對點提供 8個約束方程就可以求解，
thread方法比較特殊，在傳遞參考的時候，外層需要用ref來進行參考傳遞，否則就是淺拷貝
一些重要引數
FindHomography 該執行緒的主函式 ref(vbMatchesInliersH), 輸出，特征點對的Inlier標記 ref(SH), 輸出，計算的單應矩陣的RANSAC評分 ref(H)); 輸出，計算的單應矩陣結果 @param[in & out] vbMatchesInliers 標記是否是外點 @param[in & out] score 計算單應矩陣的得分 @param[in & out] H21 單應矩陣結果
具體步驟
計算單應矩陣，假設場景為平面情況下通過前兩幀求取Homography矩陣，并得到該模型的評分
原理參考Multiple view geometry in computer vision P109 演算法4.4
* Step 1 將當前幀和參考幀中的特征點坐標進行歸一化
* Step 2 選擇8個歸一化之后的點對進行迭代
* Step 3 八點法計算單應矩陣矩陣
* Step 4 利用重投影誤差為當次RANSAC的結果評分
* Step 5 更新具有最優評分的單應矩陣計算結果,并且保存所對應的特征點對的內點標記
1、將當前幀和參考幀的特征點坐標進行歸一化（對應函式Initializer::Normalize）
原理參考
Multiple view geometry in computer vision P109 演算法4.4
Data normalization is an essential step in the DLT algorithm. It must not be considered optional. Data normalization becomes even more important for less well conditioned problems, such as the DLT computation of the fundamental matrix or the trifocal tensor, which will be considered in later chapters
為什么要歸一化
Ah=0
矩陣A是利用8點法求基礎矩陣的關鍵，所以Hartey就認為，利用8點法求基礎矩陣不穩定的一個主要原因就是原始的影像像點坐標組成的系數矩陣A不好造成的，而造成A不好的原因是像點的齊次坐標各個分量的數量級相差太大，基于這個原因，Hartey提出一種改進的8點法，在應用8點法求基礎矩陣之前，先對像點坐標進行歸一化處理，即對原始的影像坐標做同向性變換，這樣就可以減少噪聲的干擾，大大的提高8點法的精度，
預先對影像坐標進行歸一化有以下好處：
能夠提高運算結果的精度
利用歸一化處理后的影像坐標，對任何尺度縮放和原點的選擇是不變的，歸一化步驟預先為影像坐 標選擇了一個標準的坐標系中，消除了坐標變換對結果的影響，
歸一化操作分兩步進行，首先對每幅影像中的坐標進行平移（每幅影像的平移不同）使影像中匹配的 組成的點集的形心（Centroid）移動到原點；接著對坐標系進行縮放使得各個分量總體上有一樣的平均值，各個坐標軸的縮放相同的，
注：使用歸一化的坐標雖然能一定程度的消除噪聲、錯誤匹配帶來的影響，但還是不夠的，
歸一化具體操作
一階矩就是隨機變數的期望，二階矩就是隨機變數平方的期望；一階絕對矩定義為變數與均值絕對值的平均，
將當前幀和參考幀中的特征點坐標進行歸一化，主要是平移和尺度變換 ，具體來說,就是將mvKeys1和mvKey2歸一化到均值為0，一階絕對矩為1，歸一化矩陣分別為T1、T2 ，這里所謂的一階絕對矩其實就是隨機變數到取值的中心的絕對值的平均值 ，歸一化矩陣就是把上述歸一化的操作用矩陣來表示，這樣特征點坐標乘歸一化矩陣可以得到歸一化后的坐標
//歸一化后的參考幀1和當前幀2中的特征點坐標
vector<cv::Point2f> vPn1, vPn2;
// 記錄各自的歸一化矩陣
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
//這里求的逆在后面的代碼中要用到，輔助進行原始尺度的恢復
cv::Mat T2inv = T2.inv();2、選擇8個歸一化的點進行迭代
for(size_t j=0; j<8; j++)
{
//從mvSets中獲取當前次迭代的某個特征點對的索引資訊
int idx = mvSets[it][j];
// vPn1i和vPn2i為匹配的特征點對的歸一化后的坐標
// 首先根據這個特征點對的索引資訊分別找到兩個特征點在各自影像特征點向量中的索引，然后讀取其歸一化之后的特征點坐標
vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存盤在參考幀1中的特征點索引
vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存盤在參考幀1中的特征點索引
}//讀取8對特征點的歸一化之后的坐標3、利用生成的8個歸一化特征點對, 呼叫函式 Initializer::ComputeH21() 使用八點法計算單應矩陣
單應矩陣原理：X2=H21*X1，其中X1,X2 為歸一化后的特征點
特征點歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2 得到:T2 * mvKeys2 = Hn * T1 * mvKeys1
進一步得到:mvKeys2 = T2.inv * Hn * T1 * mvKeys1
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
H21i = T2inv*Hn*T1;
//然后計算逆
H12i = H21i.inv();4、利用重投影誤差為當次RANSAC的結果評分
RANSAC演算法
少數外點會極大影響計算結果的準確度.隨著采樣數量的增加,外點數量也會同時增加,這是一種系統誤差,無法通過增加采樣點來解決.
RANSAC(Random sample consensus,隨機采樣一致性)演算法的思路是少量多次重復實驗,每次實驗僅使用盡可能少的點來計算,并統計本次計算中的內點數.只要嘗試次數足夠多的話,總會找到一個包含所有內點的解.
RANSAC演算法的核心是減少每次迭代所需的采樣點數.從原理上來說,計算F矩陣最少只需要7對匹配點,計算H矩陣最少只需要4對匹配點;ORB-SLAM2中為了編程方便,每次迭代使用8對匹配點計算F和H.
currentScore = CheckHomography(H21i, H12i, //輸入，單應矩陣的計算結果
vbCurrentInliers, //輸出，特征點對的Inliers標記
mSigma); //TODO 測量誤差，在Initializer類物件構造的時候，由外部給定的
5、更新具有最優評分的單應矩陣計算結果,并且保存所對應的特征點對的內點標記
if(currentScore>score)
{
//如果當前的結果得分更高，那么就更新最優計算結果
H21 = H21i.clone();
//保存匹配好的特征點對的Inliers標記
vbMatchesInliers = vbCurrentInliers;
//更新歷史最優評分
score = currentScore;
}計算基礎矩陣，假設場景為非平面情況下通過前兩幀求取Fundamental矩陣，得到該模型的評分
等式左邊兩項分別用A, f表示，則有
Af=0
一對點提供一個約束方程，基礎矩陣F總共有9個元素，7個自由度（尺度等價性，秩為2），所以8對點 提供8個約束方程就可以求解F，
求解基礎矩陣F和求解單應矩陣類似，我們就不一一展開了，
八點法計算基礎矩陣
基礎矩陣約束：p2^t*F21*p1 = 0，其中p1,p2 為齊次化特征點坐標
特征點歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2
根據基礎矩陣約束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0
進一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
F21i = T2t*Fn*T1;
SVD
SVD分解結果
假設我們使用8對點求解，A 是 8x9 矩陣，分解后 U 是左奇異向量，它是一個8x8的 正交矩陣， V 是右奇異向量，是一個 9x9 的正交矩陣， 是V的轉置 D是一個8 x 9 對角矩陣，除了對角線其他元素均為0，對角線元素稱為奇異值，一般來說奇異值是按照 從大到小的順序降序排列，因為每個奇異值都是一個殘差項，因此最后一個奇異值最小，其含義就是最 優的殘差，因此其對應的奇異值向量就是最優值，即最優解，
中的每個列向量對應著D中的每個奇異值，最小二乘最優解就是 對應的第9個列向量，也就是基礎 矩陣F的元素，這里我們先記做 Fpre，因為這個還不是最終的F
F矩陣秩為2
基礎矩陣 F 有個很重要的性質，就是秩為2，可以進一步約束求解準確的F ，上面的方法使用 對應的第9個列向量構造的Fpre 秩通常不為2，我們可以繼續進行SVD分解，
其最小奇異值人為置為0，這樣F矩陣秩為2
此時的F就是最終得到的基礎矩陣，
使用opencv提供的進行奇異值分解的函式
cv::SVDecomp(A, //輸入，待進行奇異值分解的矩陣
w, //輸出，奇異值矩陣
u, //輸出，矩陣U
vt, //輸出，矩陣V^T
cv::SVD::MODIFY_A | //輸入，MODIFY_A是指允許計算函式可以修改待分解的矩陣，官方檔案上說這樣可以加快計算速度、節省記憶體
cv::SVD::FULL_UV); //FULL_UV=把U和VT補充成單位正交方陣
// 回傳最小奇異值所對應的右奇異向量
// 注意前面說的是右奇異值矩陣的最后一列，但是在這里因為是vt，轉置后了，所以是行；由于A有9列資料，故最后一列的下標為8
return vt.row(8).reshape(0, //轉換后的通道數，這里設定為0表示是與前面相同
3); //轉換后的行數,對應V的最后一列
}
cv::Mat Initializer::ComputeF21(
const vector<cv::Point2f> &vP1, //歸一化后的點, in reference frame
const vector<cv::Point2f> &vP2) //歸一化后的點, in current frame
{
// 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最小特征值對應的特征向量即為解
//獲取參與計算的特征點對數
const int N = vP1.size();
//初始化A矩陣
cv::Mat A(N,9,CV_32F); // N*9維
// 構造矩陣A，將每個特征點添加到矩陣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;
}
//存盤奇異值分解結果的變數
cv::Mat u,w,vt;
// 定義輸出變數，u是左邊的正交矩陣U， w為奇異矩陣，vt中的t表示是右正交矩陣V的轉置
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
// 轉換成基礎矩陣的形式
cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
//基礎矩陣的秩為2,而我們不敢保證計算得到的這個結果的秩為2,所以需要通過第二次奇異值分解,來強制使其秩為2
// 對初步得來的基礎矩陣進行第2次奇異值分解
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
// 秩2約束，強制將第3個奇異值設定為0
w.at<float>(2)=0;
// 重新組合好滿足秩約束的基礎矩陣，作為最終計算結果回傳
return u*cv::Mat::diag(w)*vt;
}
四、對給定的homography matrix打分,需要使用到卡方檢驗的知識
卡方檢驗
為什么要參考卡方檢驗?
以特定概率分布為某種情況建模時，事物長期結果較為穩定，能夠清晰進行把握，比如拋硬幣實驗， 但是期望與事實存在差異怎么辦？偏差是正常的小幅度波動？還是建模錯誤？此時，利用卡方分布分析 結果，排除可疑結果，
簡單來說：當事實與期望不符合情況下使用卡方分布進行檢驗，看是否系統出了問題，還是屬于正常波動
卡方分布用途
檢查實際結果與期望結果之間何時存在顯著差異，
1、檢驗擬合程度：也就是說可以檢驗一組給定資料與指定分布的吻合程度，如：用它檢驗抽獎機收益的 觀察頻數與我們所期望的吻合程度，
2、檢驗兩個變數的獨立性：通過這個方法檢查變數之間是否存在某種關系，
卡方分布假設檢驗步驟
1、確定要進行檢驗的假設（H0）及其備擇假設H1.
2、求出期望E.
3、確定用于做決策的拒絕域（右尾）.
4、根據自由度和顯著性水平查詢檢驗統計量臨界值.
5、查看檢驗統計量是否在拒絕域內.
6、做出決策.
根據自由度和顯著性水平查詢檢驗統計量臨界值
自由度的影響
自由度:用于計算檢驗統計量的獨立變數的數目，
當自由度等于1或者2時：卡方分布先高后低的平滑曲線，檢驗統計量等于較小值的概率遠遠大于較大值 的概率，即觀察頻數有可能接近期望頻數，
當自由度大于2時：卡方分布先低后高再低，其外形沿著正向扭曲，但當自由度很大時，圖形接近正態分布，
自由度的計算， 對于單行或單列：自由度 = 組數-限制數 對于表格類：自由度 = (行數 - 1) * (列數 - 1)
決策原則
如果位于拒絕域內我們拒絕原假設H0，接受H1，
如果不在拒絕域內我們接受原假設H0，拒絕H1
檢驗統計量38.272 > 9.49 位于拒絕域內 于是拒絕原假設：抽獎機每局收益如何概率分布，也就是說抽獎機被人動了手腳
檢驗統計量拒絕域內外判定：
1、求出檢驗統計量a
2、通過自由度和顯著性水平查到拒絕域臨界值b
3、a>b則位于拒絕域內，反之，位于拒絕域外，
const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
const float invz1 = 1.0/z1;
if(!bStereo1)
{
// 單目情況下
float u1 = fx1*x1*invz1+cx1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
// 假設測量有一個像素的偏差，2自由度卡方檢驗閾值是5.991
if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
continue;
}
else
{
// 雙目情況
float u1 = fx1*x1*invz1+cx1;
// 根據視差公式計算假想的右目坐標
float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
// 自由度為3，卡方檢驗閾值是7.8
if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
continue;
}五、從基礎矩陣F中求解位姿R，t及三維點
F分解出E，E有四組解，選擇計算的有效三維點（在攝像頭前方、投影誤差小于閾值、視差角大于閾值）最多的作為最優的解
一些重要引數
@param[in] vbMatchesInliers 匹配好的特征點對的Inliers標記 * @param[in] F21 從參考幀到當前幀的基礎矩陣 * @param[in] K 相機的內引數矩陣 * @param[in & out] R21 計算好的相機從參考幀到當前幀的旋轉 * @param[in & out] t21 計算好的相機從參考幀到當前幀的平移 * @param[in & out] vP3D 三角化測量之后的特征點的空間坐標 * @param[in & out] vbTriangulated 特征點三角化成功的標志 * @param[in] minParallax 認為三角化有效的最小視差角 * @param[in] minTriangulated 最小三角化點數量 * @return true 成功初始化 * @return false 初始化失敗
1、統計有效匹配點個數，并用 N 表示
2、根據基礎矩陣和相機的內引數矩陣計算本質矩陣
定義本質矩陣分解結果，形成四組解,分別是： (R1, t) (R1, -t) (R2, t) (R2, -t)
3、從本質矩陣求解兩個R解和兩個t解，共四組解
4、分別驗證求解的4種R和t的組合，選出最佳組合
六、用H矩陣恢復R, t和三維點
H矩陣分解常見有兩種方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition ， 代碼使用了Faugeras SVD-based decomposition演算法，
一些重要引數
* @param[in] vbMatchesInliers 匹配點對的內點標記 * @param[in] H21 從參考幀到當前幀的單應矩陣 * @param[in] K 相機的內引數矩陣 * @param[in & out] R21 計算出來的相機旋轉 * @param[in & out] t21 計算出來的相機平移 * @param[in & out] vP3D 世界坐標系下，三角化測量特征點對之后得到的特征點的空間坐標 * @param[in & out] vbTriangulated 特征點是否成功三角化的標記 * @param[in] minParallax 對特征點的三角化測量中，認為其測量有效時需要滿足的最小視差角（如果視差角過小則會引起非常大的觀測誤差）,單位是角度 * @param[in] minTriangulated 為了進行運動恢復，所需要的最少的三角化測量成功的點個數 * @return true 單應矩陣成功計算出位姿和三維點 * @return false 初始化失敗
坐標
流程:
1. 根據H矩陣的奇異值d'= d2 或者 d' = -d2 分別計算 H 矩陣分解的 8 組解
1.1 討論 d' > 0 時的 4 組解
1.2 討論 d' < 0 時的 4 組解
2. 對 8 組解進行驗證，并選擇產生相機前方最多3D點的解為最優解
轉載請註明出處，本文鏈接：https://www.uj5u.com/ruanti/298696.html
標籤：其他