基本流程

??單目初始化程序存儲在Initializer.cc中
??需要注意，對于雙目/RGB-D相機，初始化時，由于可以直接獲得相機的深度信息，因此無需求H/F，直接作為關鍵幀插入就行。
??使用RANSAC+DLT求解H，RANSAC+八點法求解F。選擇評分高的，然后進行分解獲得R，t。分解時會有多組解，選擇最多深度為正的路標點的解。

?
?

構建初始化器

nitializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)

?
?

特征點坐標歸一化處理

??前輩發現計算單應矩陣時變換特征點的坐標會得到更好的效果，包括坐標的平移和尺度縮放，并且這一步驟必須放在DLT之前。DLT之后再還原到原坐標系。

（1）將點進行平移使其形心（x,y的均值）位于原點。
（2）對點進行縮放使特征點到原點的距離為根號2，即所有點“平均”位于（1，1，1）
（3）對兩幅圖進行獨立的上述變換

?
?

計算單應矩陣

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, //歸一化后的點, in reference frameconst vector<cv::Point2f> &vP2) //歸一化后的點, in current frame

??有關SVD分解過程可以參考
https://blog.csdn.net/sinat_28309919/article/details/80134985
??奇異值分解后，V的最后一列向量即為解。

    // 定義輸出變量，u是左邊的正交矩陣U， w為奇異矩陣，vt中的t表示是右正交矩陣V的轉置cv::Mat u,w,vt;//使用opencv提供的進行奇異值分解的函數cv::SVDecomp(A,							//輸入，待進行奇異值分解的矩陣w,							//輸出，奇異值矩陣u,							//輸出，矩陣Uvt,						//輸出，矩陣V^Tcv::SVD::MODIFY_A | 		//輸入，MODIFY_A是指允許計算函數可以修改待分解的矩陣，官方文檔上說這樣可以加快計算速度、節省內存cv::SVD::FULL_UV);		//FULL_UV=把U和VT補充成單位正交方陣// 返回最小奇異值所對應的右奇異向量// 注意前面說的是右奇異值矩陣的最后一列，但是在這里因為是vt，轉置后了，所以是行；由于A有9列數據，故最后一列的下標為8return vt.row(8).reshape(0, 			//轉換后的通道數，這里設置為0表示是與前面相同3); 			//轉換后的行數,對應V的最后一列

?
?

計算H的評分

??使用對稱轉移誤差為評判標準。據此來判斷特征點是內點還是外點。

        currentScore = CheckHomography(H21i, H12i, 			//輸入，單應矩陣的計算結果vbCurrentInliers, 	//輸出，特征點對的Inliers標記mSigma);	

通過H矩陣，進行參考幀和當前幀之間的雙向投影，并計算起加權最小二乘投影誤差

（1）計算重投影誤差：

        const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 = squareDist1 * invSigmaSquare;

（2）判斷是內點還是外點
??誤差小于閾值的話，為內點。內點的評分與誤差的大小有關，誤差越大，評分越小。

        // Step 2.3 用閾值標記離群點，內點的話累加得分if(chiSquare1>th)bIn = false;    else// th為設定的閾值，誤差越大，得分越低score += th - chiSquare1;

設定內點：

        // Step 2.4 如果從img2 到 img1 和 從img1 到img2的重投影誤差均滿足要求，則說明是Inlier pointif(bIn)vbMatchesInliers[i]=true;elsevbMatchesInliers[i]=false;

?
?

尋找單應矩陣

void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)

對特征點坐標進行歸一化處理

??將當前幀和參考幀中的特征點坐標進行歸一化。

	//歸一化后的參考幀1和當前幀2中的特征點坐標vector<cv::Point2f> vPn1, vPn2;// 記錄各自的歸一化矩陣cv::Mat T1, T2;Normalize(mvKeys1,vPn1, T1);Normalize(mvKeys2,vPn2, T2);

取出RANSAC獲得的隨機特征點對的索引

        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對特征點的歸一化之后的坐標

計算單應矩陣

?&emps;需要注意，這里計算的單應矩陣使用的是歸一化坐標。然后要恢復到原始坐標下。

        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);// 單應矩陣原理：X2=H21*X1，其中X1,X2 為歸一化后的特征點    // 特征點歸一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  得到:T2 * mvKeys2 =  Hn * T1 * mvKeys1   // 進一步得到:mvKeys2  = （T2.inv * Hn * T1） * mvKeys1H21i = T2inv*Hn*T1;//然后計算逆H12i = H21i.inv();

計算該次迭代獲得的H的評分

        // Step 4 利用重投影誤差為當次RANSAC的結果評分currentScore = CheckHomography(H21i, H12i, 			//輸入，單應矩陣的計算結果vbCurrentInliers, 	//輸出，特征點對的Inliers標記mSigma);	

保存最優的H

        // Step 5 更新具有最優評分的單應矩陣計算結果,并且保存所對應的特征點對的內點標記if(currentScore>score){//如果當前的結果得分更高，那么就更新最優計算結果H21 = H21i.clone();//保存匹配好的特征點對的Inliers標記vbMatchesInliers = vbCurrentInliers;//更新歷史最優評分score = currentScore;}

?
?

計算基礎矩陣

cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //歸一化后的點, in reference frameconst vector<cv::Point2f> &vP2) //歸一化后的點, in current frame

SVD分解，D的最后一列即為F的值

    //存儲奇異值分解結果的變量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個奇異值設置為0w.at<float>(2)=0;// 重新組合好滿足秩約束的基礎矩陣，作為最終計算結果返回 return  u*cv::Mat::diag(w)*vt;

?
?

計算F的評分

??誤差為像素點到極線的距離。

通過點到極線的距離計算誤差

??誤差小于閾值的話，設為內點。誤差越小，貢獻越大。

        // Step 2.2 計算 img1 上的點在 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 計算誤差 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);// 帶權重誤差const float chiSquare1 = squareDist1*invSigmaSquare;// Step 2.4 誤差大于閾值就說明這個點是Outlier // ? 為什么判斷閾值用的 th（1自由度），計算得分用的thScore（2自由度）// ? 可能是為了和CheckHomography 得分統一？if(chiSquare1>th)bIn = false;else// 誤差越大，得分越低score += thScore - chiSquare1;

尋找基礎矩陣

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)

取出RANSAC獲得的特征點索引

        for(int j=0; j<8; j++){int idx = mvSets[it][j];// vPn1i和vPn2i為匹配的特征點對的歸一化后的坐標// 首先根據這個特征點對的索引信息分別找到兩個特征點在各自圖像特征點向量中的索引，然后讀取其歸一化之后的特征點坐標vPn1i[j] = vPn1[mvMatches12[idx].first];        //first存儲在參考幀1中的特征點索引vPn2i[j] = vPn2[mvMatches12[idx].second];       //second存儲在參考幀1中的特征點索引}

使用八點法計算基礎矩陣F

        // Step 3 八點法計算基礎矩陣cv::Mat Fn = ComputeF21(vPn1i,vPn2i);// 基礎矩陣約束：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 = 0F21i = T2t*Fn*T1;

獲得該基礎矩陣的評分

        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

更新最優的基礎矩陣

        if(currentScore>score){//如果當前的結果得分更高，那么就更新最優計算結果F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}

?
?

初始化主函數

Tracking.cc的947行
（1）匹配特征點
（2）獲得基礎矩陣與單應矩陣
（3）選擇最佳的來恢復兩幀間的位姿變換。
（4）三角化獲得路標點

/*** @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*/bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)

獲得兩幀間的特征點匹配關系

    for(size_t i=0, iend=vMatches12.size();i<iend; i++){//vMatches12[i]解釋：i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值//沒有匹配關系的話，vMatches12[i]值為 -1if(vMatches12[i]>=0){//mvMatches12 中只記錄有匹配關系的特征點對的索引值//i表示幀1中關鍵點的索引值，vMatches12[i]的值為幀2的關鍵點索引值mvMatches12.push_back(make_pair(i,vMatches12[i]));//標記參考幀1中的這個特征點有匹配關系mvbMatched1[i]=true;}else//標記參考幀1中的這個特征點沒有匹配關系mvbMatched1[i]=false;}

在所有匹配的特征點中隨機取出8組點，求解單應矩陣與基礎矩陣

（1）獲得RANSAC算法中，每次進行計算的8組點的索引
mvSets：迭代次數索引—>每次迭代時特征點對的索引

    for(int it=0; it<mMaxIterations; it++){//迭代開始的時候，所有的點都是可用的vAvailableIndices = vAllIndices;// Select a minimum set//選擇最小的數據樣本集，使用八點法求，所以這里就循環了八次for(size_t j=0; j<8; j++){// 隨機產生一對點的id,范圍從0到N-1int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);// idx表示哪一個索引對應的特征點對被選中int idx = vAvailableIndices[randi];//將本次迭代這個選中的第j個特征點對的索引添加到mvSets中mvSets[it][j] = idx;// 由于這對點在本次迭代中已經被使用了,所以我們為了避免再次抽到這個點,就在"點的可選列表"中,// 將這個點原來所在的位置用vector最后一個元素的信息覆蓋,并且刪除尾部的元素// 這樣就相當于將這個點的信息從"點的可用列表"中直接刪除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出8個特征點對}//迭代mMaxIterations次，選取各自迭代時需要用到的最小數據集

（2）開兩個線程，分別計算基礎矩陣和單應矩陣

    // 構造線程來計算H矩陣及其得分// thread方法比較特殊，在傳遞引用的時候，外層需要用ref來進行引用傳遞，否則就是淺拷貝thread threadH(&Initializer::FindHomography,	//該線程的主函數this,							//由于主函數為類的成員函數，所以第一個參數就應該是當前對象的this指針ref(vbMatchesInliersH), 			//輸出，特征點對的Inlier標記ref(SH), 						//輸出，計算的單應矩陣的RANSAC評分ref(H));							//輸出，計算的單應矩陣結果// 計算fundamental matrix并打分，參數定義和H是一樣的，這里不再贅述thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));// Wait until both threads have finished//等待兩個計算線程結束threadH.join();threadF.join();

?
?

根據評分選擇F/ H，恢復R，t

??更傾向于選擇單應矩陣。因為單應矩陣對低視差的容忍程度比基礎矩陣高。

    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矩陣恢復位姿。如果單應矩陣的評分占比達到了0.4以上,則從單應矩陣恢復運動,否則從基礎矩陣恢復運動if(RH>0.40)//更偏向于平面，此時從單應矩陣恢復，函數ReconstructH返回bool型結果return ReconstructH(vbMatchesInliersH,	//輸入，匹配成功的特征點對Inliers標記H,					//輸入，前面RANSAC計算后的單應矩陣mK,					//輸入，相機的內參數矩陣R21,t21,			//輸出，計算出來的相機從參考幀1到當前幀2所發生的旋轉和位移變換vP3D,				//特征點對經過三角測量之后的空間坐標，也就是地圖點vbTriangulated,		//特征點對是否成功三角化的標記1.0,				//這個對應的形參為minParallax，即認為某對特征點的三角化測量中，認為其測量有效時//需要滿足的最小視差角（如果視差角過小則會引起非常大的觀測誤差）,單位是角度50);				//為了進行運動恢復，所需要的最少的三角化測量成功的點個數else //if(pF_HF>0.6)// 更偏向于非平面，從基礎矩陣恢復return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

?
?

從F中恢復R,t

根據F矩陣獲得E矩陣

cv::Mat E21 = K.t()*F21*K;

分解E矩陣，獲得4組解

    DecomposeE(E21,R1,R2,t);  cv::Mat t1=t;cv::Mat t2=-t;

選擇最佳的R，t解

??若某一組合使恢復得到的3D點位于相機正前方的數量最多，那么該組合就是最佳組合。**E矩陣分解有4中可能，H矩陣分解有8種可能。**在這個過程中，會進行三角化。

（1）選出最合適的R，t解（內點在相機的前面，有足夠的視角）
（2）4組解中，如果最優的R，t解不夠突出，就放棄。

    int nGood1 = CheckRT(R1,t1,							//當前組解mvKeys1,mvKeys2,				//參考幀和當前幀中的特征點mvMatches12, vbMatchesInliers,	//特征點的匹配關系和Inliers標記K, 							//相機的內參數矩陣vP3D1,							//存儲三角化以后特征點的空間坐標4.0*mSigma2,					//三角化測量過程中允許的最大重投影誤差vbTriangulated1,				//參考幀中被成功進行三角化測量的特征點的標記parallax1);					//認為某對特征點三角化測量有效的比較大的視差角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);

保存分解結果

??包括保存最佳R，t、三角化后的點的坐標等。

?
?

三角化

void Initializer::Triangulate(const cv::KeyPoint &kp1,    //特征點, in reference frameconst cv::KeyPoint &kp2,    //特征點, in current frameconst cv::Mat &P1,          //投影矩陣P1（K*T）const cv::Mat &P2,          //投影矩陣P2cv::Mat &x3D)               //三維點

??進行SVD分解，D的列向量即為AX=0的解X。隨后對X進行歸一化處理。

	//構造參數矩陣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);//奇異值分解的結果cv::Mat u,w,vt;//對系數矩陣A進行奇異值分解cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);//根據前面的結論，奇異值分解右矩陣的最后一行其實就是解，原理類似于前面的求最小二乘解，四個未知數四個方程正好正定//別忘了我們更習慣用列向量來表示一個點的空間坐標x3D = vt.row(3).t();//為了符合其次坐標的形式，使最后一維為1x3D = x3D.rowRange(0,3)/x3D.at<float>(3);

?
?

三角化

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)

遍歷特征點對，進行三角化

    for(size_t i=0, iend=vMatches12.size();i<iend;i++){// 跳過outliersif(!vbMatchesInliers[i])continue;// Step 2 獲取特征點對，調用Triangulate() 函數進行三角化，得到三角化測量之后的3D點坐標// kp1和kp2是匹配好的有效特征點const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];//存儲三維點的的坐標cv::Mat p3dC1;// 利用三角法恢復三維點p3dC1Triangulate(kp1,kp2,	//特征點P1,P2,		//投影矩陣p3dC1);		//輸出，三角化測量之后特征點的空間坐標	.....................}

檢查三角化后的路標點

（1）三角化后的路標點的X，Y，Z不能無限大。

        if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))){//其實這里就算是不這樣寫也沒問題，因為默認的匹配點對就不是good點vbGood[vMatches12[i].first]=false;//繼續對下一對匹配點的處理continue;}

（2）Z大于0，且視差不能太小

        //得到向量PO1cv::Mat normal1 = p3dC1 - O1;//求取模長，其實就是距離float dist1 = cv::norm(normal1);//同理構造向量PO2cv::Mat normal2 = p3dC1 - O2;//求模長float dist2 = cv::norm(normal2);//根據公式：a.*b=|a||b|cos_theta 可以推導出來下面的式子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)// 如果深度值為負值，為非法三維點跳過該匹配點對// ?視差比較小時，重投影誤差比較大。這里0.99998 對應的角度為0.36°,這里不應該是 cosParallax>0.99998 嗎？// ?因為后面判斷vbGood 點時的條件也是 cosParallax<0.99998 // !可能導致初始化不穩定if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)continue;

（3）路標點投影到圖像中，重投影誤差不能太大

        float im1x, im1y;//這個使能空間點的z坐標的倒數float invZ1 = 1.0/p3dC1.at<float>(2);//投影到參考幀圖像上。因為參考幀下的相機坐標系和世界坐標系重合，因此這里就直接進行投影就可以了im1x = fx*p3dC1.at<float>(0)*invZ1+cx;im1y = fy*p3dC1.at<float>(1)*invZ1+cy;//參考幀上的重投影誤差，這個的確就是按照定義來的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// 計算3D點在第二個圖像上的投影誤差，計算過程和第一個圖像類似float im2x, im2y;// 注意這里的p3dC2已經是第二個相機坐標系下的三維點了float invZ2 = 1.0/p3dC2.at<float>(2);im2x = fx*p3dC2.at<float>(0)*invZ2+cx;im2y = fy*p3dC2.at<float>(1)*invZ2+cy;// 計算重投影誤差float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);// 重投影誤差太大，跳過淘汰if(squareError2>th2)continue;

取出一個較小的視差角

??在分解F矩陣獲得R、t，隨后進行三角化之后會對視差角進行判斷。如果視差角太小，說明這次平移很小，ReconstructF()函數直接返回失敗。

在這里插入代碼片

    // Step 7 得到3D點中較大的視差角，并且轉換成為角度制表示if(nGood>0){// 從小到大排序，注意vCosParallax值越大，視差越小sort(vCosParallax.begin(),vCosParallax.end());// !排序后并沒有取最小的視差角，而是取一個較小的視差角// 作者的做法：如果經過檢驗過后的有效3D點小于50個，那么就取最后那個最小的視差角(cos值最大)// 如果大于50個，就取排名第50個的較小的視差角即可，為了避免3D點太多時出現太小的視差角 size_t idx = min(50,int(vCosParallax.size()-1));//將這個選中的角弧度制轉換為角度制parallax = acos(vCosParallax[idx])*180/CV_PI;}

SVD分解E矩陣，獲得R,t

    //                          |0 -1  0|// E = U Sigma V'   let W = |1  0  0|//                          |0  0  1|// 得到4個解 E = [R|t]// R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{// 對本質矩陣進行奇異值分解//準備存儲對本質矩陣進行奇異值分解的結果cv::Mat u,w,vt;//對本質矩陣進行奇異值分解cv::SVD::compute(E,w,u,vt);// 左奇異值矩陣U的最后一列就是t，對其進行歸一化u.col(2).copyTo(t);t=t/cv::norm(t);// 構造一個繞Z軸旋轉pi/2的旋轉矩陣W，按照下式組合得到旋轉矩陣 R1 = u*W*vt//計算完成后要檢查一下旋轉矩陣行列式的數值，使其滿足行列式為1的約束cv::Mat W(3,3,CV_32F,cv::Scalar(0));W.at<float>(0,1)=-1;W.at<float>(1,0)=1;W.at<float>(2,2)=1;//計算R1 = u*W*vt;//旋轉矩陣有行列式為+1的約束，所以如果算出來為負值，需要取反if(cv::determinant(R1)<0) R1=-R1;// 同理將矩陣W取轉置來按照相同的公式計算旋轉矩陣R2 = u*W.t()*vtR2 = u*W.t()*vt;//旋轉矩陣有行列式為1的約束if(cv::determinant(R2)<0)R2=-R2;
}

總結

以上是生活随笔為你收集整理的ORB_SLAM2单目初始化策略的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。