0 引言

ORB-SLAM2算法7詳細(xì)了解了System主類和多線程、ORB-SLAM2學(xué)習(xí)筆記8詳細(xì)了解了圖像特征點(diǎn)提取和描述子的生成、ORB-SLAM2算法9詳細(xì)了解了圖像幀、ORB-SLAM2算法10詳細(xì)了解了圖像關(guān)鍵幀及ORB-SLAM2算法11詳細(xì)了解了地圖點(diǎn)，本文主要學(xué)習(xí)ORB-SLAM2中的單目初始化Initializer類。

本文用到了2D-2D對(duì)極約束，并涉及到基本矩陣、本質(zhì)矩陣和單應(yīng)矩陣的定義，及如何從本質(zhì)矩陣或單應(yīng)矩陣恢復(fù)相機(jī)運(yùn)動(dòng)的位姿R，t，比如八點(diǎn)法、奇異值分解SVD等數(shù)學(xué)原理。詳細(xì)參考如下：

👉 2D-2D對(duì)極約束中的基本矩陣、本質(zhì)矩陣和單應(yīng)矩陣

1 單目初始化Initializer

1.1 構(gòu)造函數(shù)

Initializer類的構(gòu)造函數(shù)主要用于初始化成員變量和參數(shù)設(shè)置。

構(gòu)造函數(shù)接受三個(gè)參數(shù)：

• const Frame &ReferenceFrame：參考幀。它是一個(gè)Frame類的對(duì)象，包含了參考幀的相關(guān)信息，如圖像、相機(jī)內(nèi)參等。通過參考幀的相機(jī)內(nèi)參，構(gòu)造函數(shù)將其克隆為成員變量mK，以便在后續(xù)的初始化過程中使用。

• float sigma：描述子距離閾值。在特征點(diǎn)匹配階段，如果兩個(gè)特征點(diǎn)的描述子之間的漢明距離小于閾值sigma，則認(rèn)為它們是匹配的特征點(diǎn)。

• int iterations：RANSAC迭代的次數(shù)。在運(yùn)動(dòng)估計(jì)階段，為了估計(jì)相機(jī)的初始位姿和三維點(diǎn)云，使用了RANSAC算法。iterations參數(shù)指定了RANSAC算法的迭代次數(shù)。

// 根據(jù)參考幀構(gòu)造初始化器Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
{//從參考幀中獲取相機(jī)的內(nèi)參數(shù)矩陣mK = ReferenceFrame.mK.clone();// 從參考幀中獲取去畸變后的特征點(diǎn)mvKeys1 = ReferenceFrame.mvKeysUn;//獲取估計(jì)誤差mSigma = sigma;mSigma2 = sigma*sigma;//最大迭代次數(shù)mMaxIterations = iterations;
}

1.2 成員函數(shù)

Initializer類中主要的成員函數(shù)一覽表：

成員函數(shù)	類型	定義
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)	public	計(jì)算基礎(chǔ)矩陣和單應(yīng)性矩陣，選取最佳的來恢復(fù)出最開始兩幀之間的相對(duì)姿態(tài)，并進(jìn)行三角化得到初始地圖點(diǎn)
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)	public	計(jì)算單應(yīng)矩陣，假設(shè)場(chǎng)景為平面情況下通過前兩幀求取Homography矩陣，并得到該模型的評(píng)分
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)	public	計(jì)算基礎(chǔ)矩陣，假設(shè)場(chǎng)景為非平面情況下通過前兩幀求取Fundamental矩陣，得到該模型的評(píng)分
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)	public	用DLT方法求解單應(yīng)矩陣H
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)	public	根據(jù)特征點(diǎn)匹配求fundamental matrix（normalized 8點(diǎn)法）
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)	public	對(duì)給定的homography matrix打分,需要使用到卡方檢驗(yàn)的知識(shí)
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)	public	對(duì)給定的Fundamental matrix打分,需要使用到卡方檢驗(yàn)的知識(shí)
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)	public	從基礎(chǔ)矩陣F中求解位姿R，t及三維點(diǎn)
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)	public	用單應(yīng)矩陣H矩陣恢復(fù)R, t和三維點(diǎn)
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)	public	給定投影矩陣P1,P2和圖像上的匹配特征點(diǎn)點(diǎn)kp1,kp2，從而計(jì)算三維點(diǎn)坐標(biāo)
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)	public	歸一化特征點(diǎn)到同一尺度，作為后續(xù)normalize DLT的輸入
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)	public	用位姿來對(duì)特征匹配點(diǎn)三角化，從中篩選中合格的三維點(diǎn)
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)	public	分解本質(zhì)矩陣Essential得到R,t

1.2.1 Initialize

Initialize初始化主要用到了如下的成員函數(shù)，它們的關(guān)系如下圖：

該函數(shù)包含了單目初始化類的所有流程，主要步驟如下：

1. 重新記錄特征點(diǎn)對(duì)的匹配關(guān)系；
2. 在所有匹配特征點(diǎn)對(duì)中隨機(jī)選擇8對(duì)匹配特征點(diǎn)為一組，用于估計(jì)H矩陣和F矩陣；
3. 計(jì)算fundamental 矩陣和homography 矩陣，為了加速分別開了線程計(jì)算；
4. 計(jì)算得分比例來判斷選取哪個(gè)模型來求位姿R,t，并對(duì)匹配的特征點(diǎn)進(jìn)行三角化。

輸入?yún)?shù)：

• CurrentFrame：當(dāng)前幀
• vMatches12：參考幀(1)與當(dāng)前幀(2)圖像中特征點(diǎn)的匹配關(guān)系
• R21：相機(jī)從參考幀到當(dāng)前幀的旋轉(zhuǎn)
• t21：相機(jī)從參考幀到當(dāng)前幀的平移
• vP3D：三角化測(cè)量之后的三維地圖點(diǎn)
• vbTriangulated：標(biāo)記三角化點(diǎn)是否有效，有效為true

輸出參數(shù)：

• true：該幀可以成功初始化，返回true
• false：該幀不滿足初始化條件，返回false

// 計(jì)算基礎(chǔ)矩陣和單應(yīng)矩陣，選取最佳的來恢復(fù)出最開始兩幀之間的相對(duì)姿態(tài)，并進(jìn)行三角化得到初始地圖點(diǎn)bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{// Fill structures with current keypoints and matches with reference frame// Reference Frame: 1, Current Frame: 2//獲取當(dāng)前幀的去畸變之后的特征點(diǎn)mvKeys2 = CurrentFrame.mvKeysUn;// mvMatches12記錄匹配上的特征點(diǎn)對(duì)，記錄的是幀2在幀1的匹配索引mvMatches12.clear();// 預(yù)分配空間，大小和關(guān)鍵點(diǎn)數(shù)目一致mvKeys2.size()mvMatches12.reserve(mvKeys2.size());// 記錄參考幀1中的每個(gè)特征點(diǎn)是否有匹配的特征點(diǎn)// 這個(gè)成員變量后面沒有用到，后面只關(guān)心匹配上的特征點(diǎn) 	mvbMatched1.resize(mvKeys1.size());// Step 1 重新記錄特征點(diǎn)對(duì)的匹配關(guān)系存儲(chǔ)在mvMatches12，是否有匹配存儲(chǔ)在mvbMatched1// 將vMatches12（有冗余） 轉(zhuǎn)化為 mvMatches12（只記錄了匹配關(guān)系）for(size_t i=0, iend=vMatches12.size();i<iend; i++){//vMatches12[i]解釋：i表示幀1中關(guān)鍵點(diǎn)的索引值，vMatches12[i]的值為幀2的關(guān)鍵點(diǎn)索引值//沒有匹配關(guān)系的話，vMatches12[i]值為 -1if(vMatches12[i]>=0){//mvMatches12 中只記錄有匹配關(guān)系的特征點(diǎn)對(duì)的索引值//i表示幀1中關(guān)鍵點(diǎn)的索引值，vMatches12[i]的值為幀2的關(guān)鍵點(diǎn)索引值mvMatches12.push_back(make_pair(i,vMatches12[i]));//標(biāo)記參考幀1中的這個(gè)特征點(diǎn)有匹配關(guān)系mvbMatched1[i]=true;}else//標(biāo)記參考幀1中的這個(gè)特征點(diǎn)沒有匹配關(guān)系mvbMatched1[i]=false;}// 有匹配的特征點(diǎn)的對(duì)數(shù)const int N = mvMatches12.size();// Indices for minimum set selection// 新建一個(gè)容器vAllIndices存儲(chǔ)特征點(diǎn)索引，并預(yù)分配空間vector<size_t> vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中，還可以被抽取來作為數(shù)據(jù)樣本的特征點(diǎn)對(duì)的索引，所以這里起的名字叫做可用的索引vector<size_t> vAvailableIndices;//初始化所有特征點(diǎn)對(duì)的索引，索引值0到N-1for(int i=0; i<N; i++){vAllIndices.push_back(i);}// Generate sets of 8 points for each RANSAC iteration// Step 2 在所有匹配特征點(diǎn)對(duì)中隨機(jī)選擇8對(duì)匹配特征點(diǎn)為一組，用于估計(jì)H矩陣和F矩陣// 共選擇 mMaxIterations (默認(rèn)200) 組//mvSets用于保存每次迭代時(shí)所使用的向量mvSets = vector< vector<size_t> >(mMaxIterations,		//最大的RANSAC迭代次數(shù)vector<size_t>(8,0));	//這個(gè)則是第二維元素的初始值，也就是第一維。這里其實(shí)也是一個(gè)第一維的構(gòu)造函數(shù)，第一維vector有8項(xiàng)，每項(xiàng)的初始值為0.//用于進(jìn)行隨機(jī)數(shù)據(jù)樣本采樣，設(shè)置隨機(jī)數(shù)種子DUtils::Random::SeedRandOnce(0);//開始每一次的迭代 for(int it=0; it<mMaxIterations; it++){//迭代開始的時(shí)候，所有的點(diǎn)都是可用的vAvailableIndices = vAllIndices;// Select a minimum set//選擇最小的數(shù)據(jù)樣本集，使用八點(diǎn)法求，所以這里就循環(huán)了八次for(size_t j=0; j<8; j++){// 隨機(jī)產(chǎn)生一對(duì)點(diǎn)的id,范圍從0到N-1int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);// idx表示哪一個(gè)索引對(duì)應(yīng)的特征點(diǎn)對(duì)被選中int idx = vAvailableIndices[randi];//將本次迭代這個(gè)選中的第j個(gè)特征點(diǎn)對(duì)的索引添加到mvSets中mvSets[it][j] = idx;// 由于這對(duì)點(diǎn)在本次迭代中已經(jīng)被使用了,所以我們?yōu)榱吮苊庠俅纬榈竭@個(gè)點(diǎn),就在"點(diǎn)的可選列表"中,// 將這個(gè)點(diǎn)原來所在的位置用vector最后一個(gè)元素的信息覆蓋,并且刪除尾部的元素// 這樣就相當(dāng)于將這個(gè)點(diǎn)的信息從"點(diǎn)的可用列表"中直接刪除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出8個(gè)特征點(diǎn)對(duì)}//迭代mMaxIterations次，選取各自迭代時(shí)需要用到的最小數(shù)據(jù)集// Launch threads to compute in parallel a fundamental matrix and a homography// Step 3 計(jì)算fundamental 矩陣 和homography 矩陣，為了加速分別開了線程計(jì)算 //這兩個(gè)變量用于標(biāo)記在H和F的計(jì)算中哪些特征點(diǎn)對(duì)被認(rèn)為是Inliervector<bool> vbMatchesInliersH, vbMatchesInliersF;//計(jì)算出來的單應(yīng)矩陣和基礎(chǔ)矩陣的RANSAC評(píng)分，這里其實(shí)是采用重投影誤差來計(jì)算的float SH, SF; //score for H and F//這兩個(gè)是經(jīng)過RANSAC算法后計(jì)算出來的單應(yīng)矩陣和基礎(chǔ)矩陣cv::Mat H, F; // 構(gòu)造線程來計(jì)算H矩陣及其得分// thread方法比較特殊，在傳遞引用的時(shí)候，外層需要用ref來進(jìn)行引用傳遞，否則就是淺拷貝thread threadH(&Initializer::FindHomography,	//該線程的主函數(shù)this,							//由于主函數(shù)為類的成員函數(shù)，所以第一個(gè)參數(shù)就應(yīng)該是當(dāng)前對(duì)象的this指針ref(vbMatchesInliersH), 			//輸出，特征點(diǎn)對(duì)的Inlier標(biāo)記ref(SH), 						//輸出，計(jì)算的單應(yīng)矩陣的RANSAC評(píng)分ref(H));							//輸出，計(jì)算的單應(yīng)矩陣結(jié)果// 計(jì)算fundamental matrix并打分，參數(shù)定義和H是一樣的，這里不再贅述thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));// Wait until both threads have finished//等待兩個(gè)計(jì)算線程結(jié)束threadH.join();threadF.join();// Compute ratio of scores// Step 4 計(jì)算得分比例來判斷選取哪個(gè)模型來求位姿R,t//通過這個(gè)規(guī)則來判斷誰的評(píng)分占比更多一些，注意不是簡單的比較絕對(duì)評(píng)分大小，而是看評(píng)分的占比float RH = SH/(SH+SF);			//RH=Ratio of Homography// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)// 注意這里更傾向于用H矩陣恢復(fù)位姿。如果單應(yīng)矩陣的評(píng)分占比達(dá)到了0.4以上,則從單應(yīng)矩陣恢復(fù)運(yùn)動(dòng),否則從基礎(chǔ)矩陣恢復(fù)運(yùn)動(dòng)if(RH>0.40)//更偏向于平面，此時(shí)從單應(yīng)矩陣恢復(fù)，函數(shù)ReconstructH返回bool型結(jié)果return ReconstructH(vbMatchesInliersH,	//輸入，匹配成功的特征點(diǎn)對(duì)Inliers標(biāo)記H,					//輸入，前面RANSAC計(jì)算后的單應(yīng)矩陣mK,					//輸入，相機(jī)的內(nèi)參數(shù)矩陣R21,t21,			//輸出，計(jì)算出來的相機(jī)從參考幀1到當(dāng)前幀2所發(fā)生的旋轉(zhuǎn)和位移變換vP3D,				//特征點(diǎn)對(duì)經(jīng)過三角測(cè)量之后的空間坐標(biāo)，也就是地圖點(diǎn)vbTriangulated,		//特征點(diǎn)對(duì)是否成功三角化的標(biāo)記1.0,				//這個(gè)對(duì)應(yīng)的形參為minParallax，即認(rèn)為某對(duì)特征點(diǎn)的三角化測(cè)量中，認(rèn)為其測(cè)量有效時(shí)//需要滿足的最小視差角（如果視差角過小則會(huì)引起非常大的觀測(cè)誤差）,單位是角度50);				//為了進(jìn)行運(yùn)動(dòng)恢復(fù)，所需要的最少的三角化測(cè)量成功的點(diǎn)個(gè)數(shù)else //if(pF_HF>0.6)// 更偏向于非平面，從基礎(chǔ)矩陣恢復(fù)return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);//一般地程序不應(yīng)該執(zhí)行到這里，如果執(zhí)行到這里說明程序跑飛了return false;
}

1.2.2 FindHomography

FindHomography主要用到了如下的成員函數(shù)，它們的關(guān)系如下圖：

該成員函數(shù)主要計(jì)算單應(yīng)矩陣：

1. 將當(dāng)前幀和參考幀中的特征點(diǎn)坐標(biāo)進(jìn)行歸一化；
2. 選擇8個(gè)歸一化后的點(diǎn)對(duì)進(jìn)行迭代；
3. 八點(diǎn)法計(jì)算單應(yīng)矩陣；
4. 利用重投影誤差為當(dāng)次RANSAC的結(jié)果評(píng)分；
5. 更新具有最優(yōu)評(píng)分的單應(yīng)矩陣計(jì)算結(jié)果，并且保存所對(duì)應(yīng)的特征點(diǎn)的內(nèi)點(diǎn)標(biāo)記。

// 計(jì)算單應(yīng)矩陣，假設(shè)場(chǎng)景為平面情況下通過前兩幀求取Homography矩陣，并得到該模型的評(píng)分void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{// Number of putative matches//匹配的特征點(diǎn)對(duì)總數(shù)const int N = mvMatches12.size();// Normalize coordinates// Step 1 將當(dāng)前幀和參考幀中的特征點(diǎn)坐標(biāo)進(jìn)行歸一化，主要是平移和尺度變換// 具體來說,就是將mvKeys1和mvKey2歸一化到均值為0，一階絕對(duì)矩為1，歸一化矩陣分別為T1、T2// 這里所謂的一階絕對(duì)矩其實(shí)就是隨機(jī)變量到取值的中心的絕對(duì)值的平均值// 歸一化矩陣就是把上述歸一化的操作用矩陣來表示。這樣特征點(diǎn)坐標(biāo)乘歸一化矩陣可以得到歸一化后的坐標(biāo)//歸一化后的參考幀1和當(dāng)前幀2中的特征點(diǎn)坐標(biāo)vector<cv::Point2f> vPn1, vPn2;// 記錄各自的歸一化矩陣cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);//這里求的逆在后面的代碼中要用到，輔助進(jìn)行原始尺度的恢復(fù)cv::Mat T2inv = T2.inv();// Best Results variables// 記錄最佳評(píng)分score = 0.0;// 取得歷史最佳評(píng)分時(shí),特征點(diǎn)對(duì)的inliers標(biāo)記vbMatchesInliers = vector<bool>(N,false);// Iteration variables//某次迭代中，參考幀的特征點(diǎn)坐標(biāo)vector<cv::Point2f> vPn1i(8);//某次迭代中，當(dāng)前幀的特征點(diǎn)坐標(biāo)vector<cv::Point2f> vPn2i(8);//以及計(jì)算出來的單應(yīng)矩陣、及其逆矩陣cv::Mat H21i, H12i;// 每次RANSAC記錄Inliers與得分vector<bool> vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score//下面進(jìn)行每次的RANSAC迭代for(int it=0; it<mMaxIterations; it++){// Select a minimum set// Step 2 選擇8個(gè)歸一化之后的點(diǎn)對(duì)進(jìn)行迭代for(size_t j=0; j<8; j++){//從mvSets中獲取當(dāng)前次迭代的某個(gè)特征點(diǎn)對(duì)的索引信息int idx = mvSets[it][j];// vPn1i和vPn2i為匹配的特征點(diǎn)對(duì)的歸一化后的坐標(biāo)// 首先根據(jù)這個(gè)特征點(diǎn)對(duì)的索引信息分別找到兩個(gè)特征點(diǎn)在各自圖像特征點(diǎn)向量中的索引，然后讀取其歸一化之后的特征點(diǎn)坐標(biāo)vPn1i[j] = vPn1[mvMatches12[idx].first];    //first存儲(chǔ)在參考幀1中的特征點(diǎn)索引vPn2i[j] = vPn2[mvMatches12[idx].second];   //second存儲(chǔ)在參考幀1中的特征點(diǎn)索引}//讀取8對(duì)特征點(diǎn)的歸一化之后的坐標(biāo)// Step 3 八點(diǎn)法計(jì)算單應(yīng)矩陣// 利用生成的8個(gè)歸一化特征點(diǎn)對(duì), 調(diào)用函數(shù) Initializer::ComputeH21() 使用八點(diǎn)法計(jì)算單應(yīng)矩陣  // 關(guān)于為什么計(jì)算之前要對(duì)特征點(diǎn)進(jìn)行歸一化，后面又恢復(fù)這個(gè)矩陣的尺度？// 可以在《計(jì)算機(jī)視覺中的多視圖幾何》這本書中P193頁中找到答案// 書中這里說,8點(diǎn)算法成功的關(guān)鍵是在構(gòu)造解的方稱之前應(yīng)對(duì)輸入的數(shù)據(jù)認(rèn)真進(jìn)行適當(dāng)?shù)臍w一化cv::Mat Hn = ComputeH21(vPn1i,vPn2i);// 單應(yīng)矩陣原理：X2=H21*X1，其中X1,X2 為歸一化后的特征點(diǎn)    // 特征點(diǎn)歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  得到:T2 * mvKeys2 =  Hn * T1 * mvKeys1   // 進(jìn)一步得到:mvKeys2  = T2.inv * Hn * T1 * mvKeys1H21i = T2inv*Hn*T1;//然后計(jì)算逆H12i = H21i.inv();// Step 4 利用重投影誤差為當(dāng)次RANSAC的結(jié)果評(píng)分currentScore = CheckHomography(H21i, H12i, 			//輸入，單應(yīng)矩陣的計(jì)算結(jié)果vbCurrentInliers, 	//輸出，特征點(diǎn)對(duì)的Inliers標(biāo)記mSigma);				//TODO  測(cè)量誤差，在Initializer類對(duì)象構(gòu)造的時(shí)候，由外部給定的// Step 5 更新具有最優(yōu)評(píng)分的單應(yīng)矩陣計(jì)算結(jié)果,并且保存所對(duì)應(yīng)的特征點(diǎn)對(duì)的內(nèi)點(diǎn)標(biāo)記if(currentScore>score){//如果當(dāng)前的結(jié)果得分更高，那么就更新最優(yōu)計(jì)算結(jié)果H21 = H21i.clone();//保存匹配好的特征點(diǎn)對(duì)的Inliers標(biāo)記vbMatchesInliers = vbCurrentInliers;//更新歷史最優(yōu)評(píng)分score = currentScore;}}
}

1.2.3 FindFundamental

FindFundamental主要用到了如下的成員函數(shù)，它們的關(guān)系如下圖：

該成員函數(shù)主要計(jì)算單應(yīng)矩陣：

1. 將當(dāng)前幀和參考幀中的特征點(diǎn)坐標(biāo)進(jìn)行歸一化；
2. 選擇8個(gè)歸一化之后的點(diǎn)對(duì)進(jìn)行迭代；
3. 八點(diǎn)法計(jì)算基礎(chǔ)矩陣；
4. 利用重投影誤差為當(dāng)次RANSAC的結(jié)果評(píng)分；
5. 更新具有最優(yōu)評(píng)分的基礎(chǔ)矩陣計(jì)算結(jié)果,并且保存所對(duì)應(yīng)的特征點(diǎn)對(duì)的內(nèi)點(diǎn)標(biāo)記。

// 計(jì)算基礎(chǔ)矩陣，假設(shè)場(chǎng)景為非平面情況下通過前兩幀求取Fundamental矩陣，得到該模型的評(píng)分void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{// 計(jì)算基礎(chǔ)矩陣,其過程和上面的計(jì)算單應(yīng)矩陣的過程十分相似.// Number of putative matches// 匹配的特征點(diǎn)對(duì)總數(shù)// const int N = vbMatchesInliers.size();  // !源代碼出錯(cuò)！請(qǐng)使用下面代替const int N = mvMatches12.size();// Normalize coordinates// Step 1 將當(dāng)前幀和參考幀中的特征點(diǎn)坐標(biāo)進(jìn)行歸一化，主要是平移和尺度變換// 具體來說,就是將mvKeys1和mvKey2歸一化到均值為0，一階絕對(duì)矩為1，歸一化矩陣分別為T1、T2// 這里所謂的一階絕對(duì)矩其實(shí)就是隨機(jī)變量到取值的中心的絕對(duì)值的平均值// 歸一化矩陣就是把上述歸一化的操作用矩陣來表示。這樣特征點(diǎn)坐標(biāo)乘歸一化矩陣可以得到歸一化后的坐標(biāo)vector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);// ! 注意這里取的是歸一化矩陣T2的轉(zhuǎn)置,因?yàn)榛A(chǔ)矩陣的定義和單應(yīng)矩陣不同，兩者去歸一化的計(jì)算也不相同cv::Mat T2t = T2.t();// Best Results variables//最優(yōu)結(jié)果score = 0.0;vbMatchesInliers = vector<bool>(N,false);// Iteration variables// 某次迭代中，參考幀的特征點(diǎn)坐標(biāo)vector<cv::Point2f> vPn1i(8);// 某次迭代中，當(dāng)前幀的特征點(diǎn)坐標(biāo)vector<cv::Point2f> vPn2i(8);// 某次迭代中，計(jì)算的基礎(chǔ)矩陣cv::Mat F21i;// 每次RANSAC記錄的Inliers與得分vector<bool> vbCurrentInliers(N,false);float currentScore;// Perform all RANSAC iterations and save the solution with highest score// 下面進(jìn)行每次的RANSAC迭代for(int it=0; it<mMaxIterations; it++){// Select a minimum set// Step 2 選擇8個(gè)歸一化之后的點(diǎn)對(duì)進(jìn)行迭代for(int j=0; j<8; j++){int idx = mvSets[it][j];// vPn1i和vPn2i為匹配的特征點(diǎn)對(duì)的歸一化后的坐標(biāo)// 首先根據(jù)這個(gè)特征點(diǎn)對(duì)的索引信息分別找到兩個(gè)特征點(diǎn)在各自圖像特征點(diǎn)向量中的索引，然后讀取其歸一化之后的特征點(diǎn)坐標(biāo)vPn1i[j] = vPn1[mvMatches12[idx].first];        //first存儲(chǔ)在參考幀1中的特征點(diǎn)索引vPn2i[j] = vPn2[mvMatches12[idx].second];       //second存儲(chǔ)在參考幀1中的特征點(diǎn)索引}// Step 3 八點(diǎn)法計(jì)算基礎(chǔ)矩陣cv::Mat Fn = ComputeF21(vPn1i,vPn2i);// 基礎(chǔ)矩陣約束：p2^t*F21*p1 = 0，其中p1,p2 為齊次化特征點(diǎn)坐標(biāo)    // 特征點(diǎn)歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  // 根據(jù)基礎(chǔ)矩陣約束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0   // 進(jìn)一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0F21i = T2t*Fn*T1;// Step 4 利用重投影誤差為當(dāng)次RANSAC的結(jié)果評(píng)分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);// Step 5 更新具有最優(yōu)評(píng)分的基礎(chǔ)矩陣計(jì)算結(jié)果,并且保存所對(duì)應(yīng)的特征點(diǎn)對(duì)的內(nèi)點(diǎn)標(biāo)記if(currentScore>score){//如果當(dāng)前的結(jié)果得分更高，那么就更新最優(yōu)計(jì)算結(jié)果F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}

1.2.4 ReconstructH

ReconstructH主要用到了如下的成員函數(shù)，它們的關(guān)系如下圖：

該成員函數(shù)主要用單應(yīng)矩陣恢復(fù)R，t和三維點(diǎn)，常見的兩種方法分別是Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition，ORB-SLAM2中使用了前者，直接調(diào)用的OpenCV庫中的SVD函數(shù)。

輸入/輸出參數(shù)：

• vbMatchesInliers：匹配點(diǎn)對(duì)的內(nèi)點(diǎn)標(biāo)記
• H21：從參考幀到當(dāng)前幀的單應(yīng)矩陣
• K：相機(jī)的內(nèi)參數(shù)矩陣
• R21：計(jì)算出來的相機(jī)旋轉(zhuǎn)
• t21：計(jì)算出來的相機(jī)平移
• vP3D：世界坐標(biāo)系下，三角化測(cè)量特征點(diǎn)對(duì)之后得到的特征點(diǎn)的空間坐標(biāo)
• vbTriangulated：特征點(diǎn)是否成功三角化的標(biāo)記
• minParallax：對(duì)特征點(diǎn)的三角化測(cè)量中，認(rèn)為其測(cè)量有效時(shí)需要滿足的最小視差角（如果視差角過小則會(huì)引起非常大的觀測(cè)誤差）,單位是角度
• minTriangulated：為了進(jìn)行運(yùn)動(dòng)恢復(fù)，所需要的最少的三角化測(cè)量成功的點(diǎn)個(gè)數(shù)

返回值：

• true：單應(yīng)矩陣成功計(jì)算出位姿和三維點(diǎn)
• false：初始化失敗

// 用單應(yīng)矩陣H矩陣恢復(fù)R, t和三維點(diǎn)bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{// 目的 ：通過單應(yīng)矩陣H恢復(fù)兩幀圖像之間的旋轉(zhuǎn)矩陣R和平移向量T// 參考 ：Motion and structure from motion in a piecewise plannar environment.//        International Journal of Pattern Recognition and Artificial Intelligence, 1988// https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment// 流程://      1. 根據(jù)H矩陣的奇異值d'= d2 或者 d' = -d2 分別計(jì)算 H 矩陣分解的 8 組解//        1.1 討論 d' > 0 時(shí)的 4 組解//        1.2 討論 d' < 0 時(shí)的 4 組解//      2. 對(duì) 8 組解進(jìn)行驗(yàn)證，并選擇產(chǎn)生相機(jī)前方最多3D點(diǎn)的解為最優(yōu)解// 統(tǒng)計(jì)匹配的特征點(diǎn)對(duì)中屬于內(nèi)點(diǎn)(Inlier)或有效點(diǎn)個(gè)數(shù)int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)if(vbMatchesInliers[i])N++;// We recover 8 motion hypotheses using the method of Faugeras et al.// Motion and structure from motion in a piecewise planar environment.// International Journal of Pattern Recognition and Artificial Intelligence, 1988// 參考SLAM十四講第二版p170-p171// H = K * (R - t * n / d) * K_inv// 其中: K表示內(nèi)參數(shù)矩陣//       K_inv 表示內(nèi)參數(shù)矩陣的逆//       R 和 t 表示旋轉(zhuǎn)和平移向量//       n 表示平面法向量// 令 H = K * A * K_inv// 則 A = k_inv * H * kcv::Mat invK = K.inv();cv::Mat A = invK*H21*K;// 對(duì)矩陣A進(jìn)行SVD分解// A 等待被進(jìn)行奇異值分解的矩陣// w 奇異值矩陣// U 奇異值分解左矩陣// Vt 奇異值分解右矩陣，注意函數(shù)返回的是轉(zhuǎn)置// cv::SVD::FULL_UV 全部分解// A = U * w * Vtcv::Mat U,w,Vt,V;cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);// 根據(jù)文獻(xiàn)eq(8)，計(jì)算關(guān)聯(lián)變量V=Vt.t();// 計(jì)算變量s = det(U) * det(V)// 因?yàn)閐et(V)==det(Vt), 所以 s = det(U) * det(Vt)float s = cv::determinant(U)*cv::determinant(Vt);// 取得矩陣的各個(gè)奇異值float d1 = w.at<float>(0);float d2 = w.at<float>(1);float d3 = w.at<float>(2);// SVD分解正常情況下特征值di應(yīng)該是正的，且滿足d1>=d2>=d3if(d1/d2<1.00001 || d2/d3<1.00001) {return false;}// 在ORBSLAM中沒有對(duì)奇異值 d1 d2 d3按照論文中描述的關(guān)系進(jìn)行分類討論, 而是直接進(jìn)行了計(jì)算// 定義8中情況下的旋轉(zhuǎn)矩陣、平移向量和空間向量vector<cv::Mat> vR, vt, vn;vR.reserve(8);vt.reserve(8);vn.reserve(8);// Step 1.1 討論 d' > 0 時(shí)的 4 組解// 根據(jù)論文eq.(12)有// x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))// x2 = 0// x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3))// 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))//    aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))// 則// x1 = e1 * aux1// x3 = e3 * aux2// 因?yàn)?e1,e2,e3 = 1 or -1// 所以有x1和x3有四種組合// x1 =  {aux1,aux1,-aux1,-aux1}// x3 =  {aux3,-aux3,aux3,-aux3}float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));float x1[] = {aux1,aux1,-aux1,-aux1};float x3[] = {aux3,-aux3,aux3,-aux3};// 根據(jù)論文eq.(13)有// sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2// cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 // 令  aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)// 則  sin(theta) = e1 * e3 * aux_stheta//     cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)// 因?yàn)?e1 e2 e3 = 1 or -1// 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};// 計(jì)算旋轉(zhuǎn)矩陣 R'//根據(jù)不同的e1 e3組合所得出來的四種R t的解//      | ctheta      0   -aux_stheta|       | aux1|// Rp = |    0        1       0      |  tp = |  0  |//      | aux_stheta  0    ctheta    |       |-aux3|//      | ctheta      0    aux_stheta|       | aux1|// Rp = |    0        1       0      |  tp = |  0  |//      |-aux_stheta  0    ctheta    |       | aux3|//      | ctheta      0    aux_stheta|       |-aux1|// Rp = |    0        1       0      |  tp = |  0  |//      |-aux_stheta  0    ctheta    |       |-aux3|//      | ctheta      0   -aux_stheta|       |-aux1|// Rp = |    0        1       0      |  tp = |  0  |//      | aux_stheta  0    ctheta    |       | aux3|// 開始遍歷這四種情況中的每一種for(int i=0; i<4; i++){//生成Rp，就是eq.(8) 的 R'cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);Rp.at<float>(0,0)=ctheta;Rp.at<float>(0,2)=-stheta[i];Rp.at<float>(2,0)=stheta[i];        Rp.at<float>(2,2)=ctheta;// eq.(8) 計(jì)算Rcv::Mat R = s*U*Rp*Vt;// 保存vR.push_back(R);// eq. (14) 生成tp cv::Mat tp(3,1,CV_32F);tp.at<float>(0)=x1[i];tp.at<float>(1)=0;tp.at<float>(2)=-x3[i];tp*=d1-d3;// 這里雖然對(duì)t有歸一化，并沒有決定單目整個(gè)SLAM過程的尺度// 因?yàn)镃reateInitialMapMonocular函數(shù)對(duì)3D點(diǎn)深度會(huì)縮放，然后反過來對(duì) t 有改變// eq.(8)恢復(fù)原始的tcv::Mat t = U*tp;vt.push_back(t/cv::norm(t));// 構(gòu)造法向量npcv::Mat np(3,1,CV_32F);np.at<float>(0)=x1[i];np.at<float>(1)=0;np.at<float>(2)=x3[i];// eq.(8) 恢復(fù)原始的法向量cv::Mat n = V*np;//看PPT 16頁的圖，保持平面法向量向上if(n.at<float>(2)<0)n=-n;// 添加到vectorvn.push_back(n);}// Step 1.2 討論 d' < 0 時(shí)的 4 組解float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);// cos_theta項(xiàng)float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);// 考慮到e1,e2的取值，這里的sin_theta有兩種可能的解float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};// 對(duì)于每種由e1 e3取值的組合而形成的四種解的情況for(int i=0; i<4; i++){// 計(jì)算旋轉(zhuǎn)矩陣 R'cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);Rp.at<float>(0,0)=cphi;Rp.at<float>(0,2)=sphi[i];Rp.at<float>(1,1)=-1;Rp.at<float>(2,0)=sphi[i];Rp.at<float>(2,2)=-cphi;// 恢復(fù)出原來的Rcv::Mat R = s*U*Rp*Vt;// 然后添加到vector中vR.push_back(R);// 構(gòu)造tpcv::Mat tp(3,1,CV_32F);tp.at<float>(0)=x1[i];tp.at<float>(1)=0;tp.at<float>(2)=x3[i];tp*=d1+d3;// 恢復(fù)出原來的tcv::Mat t = U*tp;// 歸一化之后加入到vector中,要提供給上面的平移矩陣都是要進(jìn)行過歸一化的vt.push_back(t/cv::norm(t));// 構(gòu)造法向量npcv::Mat np(3,1,CV_32F);np.at<float>(0)=x1[i];np.at<float>(1)=0;np.at<float>(2)=x3[i];// 恢復(fù)出原來的法向量cv::Mat n = V*np;// 保證法向量指向上方if(n.at<float>(2)<0)n=-n;// 添加到vector中vn.push_back(n);}// 最好的good點(diǎn)int bestGood = 0;// 其次最好的good點(diǎn)int secondBestGood = 0;    // 最好的解的索引，初始值為-1int bestSolutionIdx = -1;// 最大的視差角float bestParallax = -1;// 存儲(chǔ)最好解對(duì)應(yīng)的，對(duì)特征點(diǎn)對(duì)進(jìn)行三角化測(cè)量的結(jié)果vector<cv::Point3f> bestP3D;// 最佳解所對(duì)應(yīng)的，那些可以被三角化測(cè)量的點(diǎn)的標(biāo)記vector<bool> bestTriangulated;// Instead of applying the visibility constraints proposed in the WFaugeras' paper (which could fail for points seen with low parallax)// We reconstruct all hypotheses and check in terms of triangulated points and parallax// Step 2. 對(duì) 8 組解進(jìn)行驗(yàn)證，并選擇產(chǎn)生相機(jī)前方最多3D點(diǎn)的解為最優(yōu)解for(size_t i=0; i<8; i++){// 第i組解對(duì)應(yīng)的比較大的視差角float parallaxi;// 三角化測(cè)量之后的特征點(diǎn)的空間坐標(biāo)vector<cv::Point3f> vP3Di;// 特征點(diǎn)對(duì)是否被三角化的標(biāo)記vector<bool> vbTriangulatedi;// 調(diào)用 Initializer::CheckRT(), 計(jì)算good點(diǎn)的數(shù)目int nGood = CheckRT(vR[i],vt[i],                    //當(dāng)前組解的旋轉(zhuǎn)矩陣和平移向量mvKeys1,mvKeys2,                //特征點(diǎn)mvMatches12,vbMatchesInliers,   //特征匹配關(guān)系以及Inlier標(biāo)記K,                              //相機(jī)的內(nèi)參數(shù)矩陣vP3Di,                          //存儲(chǔ)三角化測(cè)量之后的特征點(diǎn)空間坐標(biāo)的4.0*mSigma2,                    //三角化過程中允許的最大重投影誤差vbTriangulatedi,                //特征點(diǎn)是否被成功進(jìn)行三角測(cè)量的標(biāo)記parallaxi);                     // 這組解在三角化測(cè)量的時(shí)候的比較大的視差角// 更新歷史最優(yōu)和次優(yōu)的解// 保留最優(yōu)的和次優(yōu)的解.保存次優(yōu)解的目的是看看最優(yōu)解是否突出if(nGood>bestGood){// 如果當(dāng)前組解的good點(diǎn)數(shù)是歷史最優(yōu)，那么之前的歷史最優(yōu)就變成了歷史次優(yōu)secondBestGood = bestGood;// 更新歷史最優(yōu)點(diǎn)bestGood = nGood;// 最優(yōu)解的組索引為i（就是當(dāng)前次遍歷）bestSolutionIdx = i;// 更新變量bestParallax = parallaxi;bestP3D = vP3Di;bestTriangulated = vbTriangulatedi;}// 如果當(dāng)前組的good計(jì)數(shù)小于歷史最優(yōu)但卻大于歷史次優(yōu)else if(nGood>secondBestGood){// 說明當(dāng)前組解是歷史次優(yōu)點(diǎn)，更新之secondBestGood = nGood;}}// Step 3 選擇最優(yōu)解。要滿足下面的四個(gè)條件// 1. good點(diǎn)數(shù)最優(yōu)解明顯大于次優(yōu)解，這里取0.75經(jīng)驗(yàn)值// 2. 視角差大于規(guī)定的閾值// 3. good點(diǎn)數(shù)要大于規(guī)定的最小的被三角化的點(diǎn)數(shù)量// 4. good數(shù)要足夠多，達(dá)到總數(shù)的90%以上if(secondBestGood<0.75*bestGood &&      bestParallax>=minParallax &&bestGood>minTriangulated && bestGood>0.9*N){// 從最佳的解的索引訪問到R，tvR[bestSolutionIdx].copyTo(R21);vt[bestSolutionIdx].copyTo(t21);// 獲得最佳解時(shí)，成功三角化的三維點(diǎn)，以后作為初始地圖點(diǎn)使用vP3D = bestP3D;// 獲取特征點(diǎn)的被成功進(jìn)行三角化的標(biāo)記vbTriangulated = bestTriangulated;//返回真，找到了最好的解return true;}return false;
}

1.2.5 ReconstructF

ReconstructF主要用到了如下的成員函數(shù)，它們的關(guān)系如下圖：

該成員函數(shù)主要用基礎(chǔ)矩陣恢復(fù)R，t和三維點(diǎn)，F分解出E，E有四組解，選擇計(jì)算的有效三維點(diǎn)（在攝像頭前方、投影誤差小于閾值、視差角大于閾值）最多的作為最優(yōu)的解。

輸入/輸出參數(shù)：

• vbMatchesInliers：匹配好的特征點(diǎn)對(duì)的Inliers標(biāo)記
• F21：從參考幀到當(dāng)前幀的基礎(chǔ)矩陣
• K：相機(jī)的內(nèi)參數(shù)矩陣
• R21：計(jì)算出來的相機(jī)旋轉(zhuǎn)
• t21：計(jì)算出來的相機(jī)平移
• vP3D：世界坐標(biāo)系下，三角化測(cè)量特征點(diǎn)對(duì)之后得到的特征點(diǎn)的空間坐標(biāo)
• vbTriangulated：特征點(diǎn)是否成功三角化的標(biāo)記
• minParallax：對(duì)特征點(diǎn)的三角化測(cè)量中，認(rèn)為其測(cè)量有效時(shí)需要滿足的最小視差角（如果視差角過小則會(huì)引起非常大的觀測(cè)誤差）,單位是角度
• minTriangulated：為了進(jìn)行運(yùn)動(dòng)恢復(fù)，所需要的最少的三角化測(cè)量成功的點(diǎn)個(gè)數(shù)

返回值：

• true：基礎(chǔ)矩陣成功計(jì)算出位姿和三維點(diǎn)
• false：初始化失敗

// 從基礎(chǔ)矩陣F中求解位姿R，t及三維點(diǎn)bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{// Step 1 統(tǒng)計(jì)有效匹配點(diǎn)個(gè)數(shù)，并用 N 表示// vbMatchesInliers 中存儲(chǔ)匹配點(diǎn)對(duì)是否是有效int N=0;for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)if(vbMatchesInliers[i]) N++;// Step 2 根據(jù)基礎(chǔ)矩陣和相機(jī)的內(nèi)參數(shù)矩陣計(jì)算本質(zhì)矩陣cv::Mat E21 = K.t()*F21*K;// 定義本質(zhì)矩陣分解結(jié)果，形成四組解,分別是：// (R1, t) (R1, -t) (R2, t) (R2, -t)cv::Mat R1, R2, t;// Step 3 從本質(zhì)矩陣求解兩個(gè)R解和兩個(gè)t解，共四組解// 不過由于兩個(gè)t解互為相反數(shù)，因此這里先只獲取一個(gè)// 雖然這個(gè)函數(shù)對(duì)t有歸一化，但并沒有決定單目整個(gè)SLAM過程的尺度. // 因?yàn)?CreateInitialMapMonocular 函數(shù)對(duì)3D點(diǎn)深度會(huì)縮放，然后反過來對(duì) t 有改變.//注意下文中的符號(hào)“'”表示矩陣的轉(zhuǎn)置//                          |0 -1  0|// E = U Sigma V'   let W = |1  0  0|//                          |0  0  1|// 得到4個(gè)解 E = [R|t]// R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3DecomposeE(E21,R1,R2,t);  cv::Mat t1=t;cv::Mat t2=-t;// Reconstruct with the 4 hyphoteses and check// Step 4 分別驗(yàn)證求解的4種R和t的組合，選出最佳組合// 原理：若某一組合使恢復(fù)得到的3D點(diǎn)位于相機(jī)正前方的數(shù)量最多，那么該組合就是最佳組合// 實(shí)現(xiàn)：根據(jù)計(jì)算的解組合成為四種情況,并依次調(diào)用 Initializer::CheckRT() 進(jìn)行檢查,得到可以進(jìn)行三角化測(cè)量的點(diǎn)的數(shù)目// 定義四組解分別在對(duì)同一匹配點(diǎn)集進(jìn)行三角化測(cè)量之后的特征點(diǎn)空間坐標(biāo)vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;// 定義四組解分別對(duì)同一匹配點(diǎn)集的有效三角化結(jié)果，True or Falsevector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;// 定義四種解對(duì)應(yīng)的比較大的特征點(diǎn)對(duì)視差角float parallax1,parallax2, parallax3, parallax4;// Step 4.1 使用同樣的匹配點(diǎn)分別檢查四組解，記錄當(dāng)前計(jì)算的3D點(diǎn)在攝像頭前方且投影誤差小于閾值的個(gè)數(shù)，記為有效3D點(diǎn)個(gè)數(shù)int nGood1 = CheckRT(R1,t1,							//當(dāng)前組解mvKeys1,mvKeys2,				//參考幀和當(dāng)前幀中的特征點(diǎn)mvMatches12, vbMatchesInliers,	//特征點(diǎn)的匹配關(guān)系和Inliers標(biāo)記K, 							//相機(jī)的內(nèi)參數(shù)矩陣vP3D1,							//存儲(chǔ)三角化以后特征點(diǎn)的空間坐標(biāo)4.0*mSigma2,					//三角化測(cè)量過程中允許的最大重投影誤差vbTriangulated1,				//參考幀中被成功進(jìn)行三角化測(cè)量的特征點(diǎn)的標(biāo)記parallax1);					//認(rèn)為某對(duì)特征點(diǎn)三角化測(cè)量有效的比較大的視差角int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);// Step 4.2 選取最大可三角化測(cè)量的點(diǎn)的數(shù)目int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));// 重置變量，并在后面賦值為最佳R和TR21 = cv::Mat();t21 = cv::Mat();// Step 4.3 確定最小的可以三角化的點(diǎn)數(shù) // 在0.9倍的內(nèi)點(diǎn)數(shù) 和 指定值minTriangulated =50 中取最大的，也就是說至少50個(gè)int nMinGood = max(static_cast<int>(0.9*N), minTriangulated);// 統(tǒng)計(jì)四組解中重建的有效3D點(diǎn)個(gè)數(shù) > 0.7 * maxGood 的解的數(shù)目// 如果有多個(gè)解同時(shí)滿足該條件，認(rèn)為結(jié)果太接近，nsimilar++，nsimilar>1就認(rèn)為有問題了，后面返回falseint nsimilar = 0;if(nGood1>0.7*maxGood)nsimilar++;if(nGood2>0.7*maxGood)nsimilar++;if(nGood3>0.7*maxGood)nsimilar++;if(nGood4>0.7*maxGood)nsimilar++;// Step 4.4 四個(gè)結(jié)果中如果沒有明顯的最優(yōu)結(jié)果，或者沒有足夠數(shù)量的三角化點(diǎn)，則返回失敗// 條件1: 如果四組解能夠重建的最多3D點(diǎn)個(gè)數(shù)小于所要求的最少3D點(diǎn)個(gè)數(shù)（mMinGood），失敗// 條件2: 如果存在兩組及以上的解能三角化出 >0.7*maxGood的點(diǎn)，說明沒有明顯最優(yōu)結(jié)果，失敗if(maxGood<nMinGood || nsimilar>1)	{return false;}//  Step 4.5 選擇最佳解記錄結(jié)果// 條件1: 有效重建最多的3D點(diǎn)，即maxGood == nGoodx，也即是位于相機(jī)前方的3D點(diǎn)個(gè)數(shù)最多// 條件2: 三角化視差角 parallax 必須大于最小視差角 minParallax，角度越大3D點(diǎn)越穩(wěn)定//看看最好的good點(diǎn)是在哪種解的條件下發(fā)生的if(maxGood==nGood1){//如果該種解下的parallax大于函數(shù)參數(shù)中給定的最小值if(parallax1>minParallax){// 存儲(chǔ)3D坐標(biāo)vP3D = vP3D1;// 獲取特征點(diǎn)向量的三角化測(cè)量標(biāo)記vbTriangulated = vbTriangulated1;// 存儲(chǔ)相機(jī)姿態(tài)R1.copyTo(R21);t1.copyTo(t21);// 結(jié)束return true;}}else if(maxGood==nGood2){if(parallax2>minParallax){vP3D = vP3D2;vbTriangulated = vbTriangulated2;R2.copyTo(R21);t1.copyTo(t21);return true;}}else if(maxGood==nGood3){if(parallax3>minParallax){vP3D = vP3D3;vbTriangulated = vbTriangulated3;R1.copyTo(R21);t2.copyTo(t21);return true;}}else if(maxGood==nGood4){if(parallax4>minParallax){vP3D = vP3D4;vbTriangulated = vbTriangulated4;R2.copyTo(R21);t2.copyTo(t21);return true;}}// 如果有最優(yōu)解但是不滿足對(duì)應(yīng)的parallax>minParallax，那么返回false表示求解失敗return false;
}

2 總結(jié)

Initializer類是用于單目相機(jī)初始化的關(guān)鍵類之一。它的主要任務(wù)是通過對(duì)初始幀進(jìn)行特征匹配和運(yùn)動(dòng)估計(jì)，來估計(jì)相機(jī)的初始位姿和場(chǎng)景的初始三維點(diǎn)云。

以下是Initializer類的主要功能和流程：

1. 在構(gòu)造函數(shù)中，Initializer類接受ORB特征提取器和描述子、相機(jī)內(nèi)參、視差閾值等參數(shù)，并進(jìn)行初始化；

2. Initializer類的核心方法是Initialize()，它接收兩個(gè)幀作為輸入：第一幀（幀1）和第二幀（幀2）。初始化過程分為兩個(gè)階段：特征匹配和運(yùn)動(dòng)估計(jì)；

3. 在特征匹配階段，Initializer使用ORB特征提取器對(duì)幀1和幀2提取特征點(diǎn)和描述子。然后，通過基于描述子的光流法進(jìn)行特征點(diǎn)匹配，得到匹配的特征點(diǎn)對(duì)；

4. 在運(yùn)動(dòng)估計(jì)階段，Initializer使用匹配的特征點(diǎn)對(duì)進(jìn)行運(yùn)動(dòng)估計(jì)。它首先計(jì)算兩個(gè)相鄰幀之間的基礎(chǔ)矩陣和單應(yīng)矩陣，然后通過基礎(chǔ)矩陣和單應(yīng)矩陣恢復(fù)出幀2相對(duì)于幀1的運(yùn)動(dòng)（旋轉(zhuǎn)和平移）；

5. 接下來，Initializer使用三角化方法對(duì)匹配的特征點(diǎn)對(duì)進(jìn)行三維重建，得到初始的稀疏三維點(diǎn)云；

6. 最后，Initializer通過對(duì)三維點(diǎn)云的篩選和剔除外點(diǎn)，以及對(duì)相機(jī)運(yùn)動(dòng)的評(píng)估，進(jìn)一步優(yōu)化初始位姿和三維點(diǎn)云。

總結(jié)來說，Initializer類通過特征匹配和運(yùn)動(dòng)估計(jì)的過程，估計(jì)相機(jī)的初始位姿和場(chǎng)景的初始三維點(diǎn)云，為ORB-SLAM2的后續(xù)跟蹤和建圖階段提供了重要的初始信息。

Reference:

• https://github.com/raulmur/ORB_SLAM2
• https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master
• 2D-2D對(duì)極約束中的基本矩陣、本質(zhì)矩陣和單應(yīng)矩陣

須知少時(shí)凌云志，曾許人間第一流。

??👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔