單目初始化 單應矩陣 本質矩陣 恢復R t 三角變換求 3D點
阿新 • • 發佈：2019-01-02
/* * This file is part of ORB-SLAM2 * * 單目相機初始化 * 用於平面場景的單應性矩陣H(8中運動假設) 和用於非平面場景的基礎矩陣F(4種運動假設)， * 然後通過一個評分規則來選擇合適的模型，恢復相機的旋轉矩陣R和平移向量t 和 對應的3D點(尺度問題)。 * * *【0】2D-2D點對 求變換矩陣前先進行標準化 去 均值後再除以絕對矩 * 單目初始化特徵點 歸一化 座標均值為0 一階絕對矩為1 * mean_x = sum( ui) / N mean_y = sum(vi)/N * 絕對矩 mean_x_dev = sum（abs(ui - mean_x)）/ N mean_y_dev = sum（abs(vi - mean_y)）/ N * * 絕對矩倒數 sX = 1/mean_x_dev sY = 1/mean_y_dev * * 標準化後的點座標 * u = (ui - mean_x) × sX * v = (vi - mean_y) * sY * * 標準化矩陣 其逆矩陣×標準化點 得到原始座標 * 用於 計算變換矩陣後 從原始座標計算對稱的轉換誤差 來計算變換矩陣得分 * T = sX 0 -mean_x * sX * 0 sY -mean_y * sY * 0 0 1 * * 標準化矩陣 * 點座標 = 標準化後的的座標 * ui ui × sX - mean_x * sX = (ui - mean_x) × sX u * T × vi = vi × sY - mean_y * sY = (vi - mean_y) × sY = v * 1 1 1 * * 點座標 = 標準化矩陣 逆矩陣 * 標準化後的的座標 * * * *【1】 2D- 2D 點對 單應矩陣 H 2D點對其次座標 間的 轉換 矩陣 3*3 * 採用歸一化的直接線性變換（normalized DLT） * * p2 = H *p1 關鍵點 對的 變換關係矩陣H * 一個點對 2個約束 * 4 點法求解 單應矩陣 H 再對 H進行分解 * *【2】 對極幾何 求解 基本矩陣 F 兩組單目相機 2D影象 * (隨機取樣序列 8點法求解) * 2D 點對 求 兩相機的 旋轉和平移矩陣 * 空間點 P 兩相機 畫素點對 p1 p2 兩相機 歸一化平面上的點對 x1 x2 與P點對應 * 相機內參數 K 兩鏡頭旋轉平移矩陣 R t 或者 變換矩陣 T * p1 = KP (世界座標系) p2 = K( RP + t) = KTP * 而 x1 = K逆* p1 x2 = K逆* p2 相機座標系下 歸一化平面上的點 x1= (px -cx)/fx x2= (py -cy)/fy * 所以 x1 = P 得到 x2 = R * x1 + t * * t 外積 x2 = t 外積 R * x1 + t 外積 t = t 外積 R * x1 ；t 外積 t =0 sin(cet) =0 垂線段投影 方向垂直兩個向量 * x2轉置 * t 外積 x2 = x2轉置 * t 外積 R x1 = 0 ；因為 t 外積 x2 得到的向量垂直 t 也垂直 x2 * 有 x2轉置 * t 外積 R x1 = x2轉置 * E * x1 = 0 ； E 為本質矩陣 * p2轉置 * K 轉置逆 * t 外積 R * K逆 * p1 = p2轉置 * F * p1 = 0 ； * F 為基礎矩陣 * * x2轉置 * E * x1 = 0 x1 x2 為 由 畫素座標轉化的歸一化座標 * 一個點對一個約束 ，8點法 可以算出 E的各個元素 ， * 再 奇異值分解 E 得到 R t * * * * 【3】變換矩陣 評分 方法 * 和卡方分佈的對應值比較，由此判定該點是否為內點。累計內點的總得分 * SM=∑i( ρM( d2cr(xic,xir,M) + ρM( d2rc(xic,xir,M ) ) * d2cr 為 2D-2D 點對 通過哦轉換矩陣的 對成轉換誤差 * * ρM 函式為 ρM(d^2) = 0 當 d^2 > 閾值(單應矩陣時 為 5.99 基礎矩陣時為 3.84) * 得分上限 - d^2 當 d^2 < 閾值 * 得分上限 均為 5.99 * *【4】 從兩個模型 H F 得分為 Sh Sf 中選著一個 最優秀的 模型 的方法為 * * 文中認為，當場景是一個平面、或近似為一個平面、或者視差較小的時候，可以使用單應性矩陣H， * 當場景是一個非平面、視差大的場景時，使用基礎矩陣F恢復運動 * RH=SH /(SH+SF) * 當RH大於0.45時，選擇從單應性變換矩陣還原運動。 * 不過ORB_SLAM2原始碼中使用的是0.4作為閾值 * * * 【5】單應矩陣求解 * * 1點 變成 2點 p2 = H21 * p1 u2 h1 h2 h3 u1 v2 = h4 h5 h6 * v1 1 h7 h8 h9 1 u2 = (h1*u1 + h2*v1 + h3) /( h7*u1 + h8*v1 + h9) v2 = (h4*u1 + h5*v1 + h6) /( h7*u1 + h8*v1 + h9) -((h4*u1 + h5*v1 + h6) - ( h7*u1*v2 + h8*v1*v2 + h9*v2))=0 式子為0 左側加 - 號不變 h1*u1 + h2*v1 + h3 - ( h7*u1*u2 + h8*v1*u2 + h9*u2)=0 0 0 0 -u1 -v1 -1 u1*v2 v1*v2 v2 u1 v1 1 0 0 0 -u1*u2 - v1*u2 -u2 ×(h1 h2 h3 h4 h5 h6 h7 h8 h9)轉置 = 0 8對點 約束 A A × h = 0 求h 奇異值分解 A 得到 單元矩陣 H * * 【6】單應變換 求 變化距離誤差 * * 1點 變成 2點 p2 = H12 * p1 u2 h11 h12 h13 u1 v2 = h21 h22 h23 * v1 1 h31 h32 h33 1 第三行 * 2 點 變成 1點 p1 = H21 * p2 u1‘ h11inv h12inv h13inv u2 v1’ = h21inv h22inv h23inv * v2 1 h31inv h32inv h33inv 1 第三行 h31inv*u2+h32inv*v2+h33inv 前兩行 同除以 第三行 消去非零因子 p2 由單應轉換到 p1 u1‘ = (h11inv*u2+h12inv*v2+h13inv)* 第三行倒數 v1’ = (h21inv*u2+h22inv*v2+h23inv)*第三行倒數 然後計算 和 真實 p1點 座標的差值 (u1-u2in1)*(u1-u2in1) + (v1-v2in1)*(v1-v2in1) 橫縱座標差值平方和 * 【7】單應矩陣恢復 旋轉矩陣 R 和平移向量t p2 = H21 * p1 p2 = K( RP + t) = KTP = H21 * KP T = K 逆 * H21*K 【8】 基礎矩陣 F求解 * * p2------> p1 * f1 f2 f3 u1 * (u2 v2 1) * f4 f5 f6 * v1 = 0 應該=0 不等於零的就是誤差 * f7 f8 f9 1 * a1 = f1*u2 + f4*v2 + f7; b1 = f2*u2 + f5*v2 + f8; c1 = f3*u2 + f6*v2 + f9; a1*u2+ b1*v2+ c1= 0 一個點對 得到一個約束方程 f1*u1*u2 + f2*v1*u2 + f3*u2 + f4*u1*v2 + f5*v1*v2 + f6*v2 + f7*u1 + f8*v1 + f9 =0 [ u1*u2 v1*u2 u2 u1*v2 v1*v2 v2 u1 v1 1 ] * [f1 f2 f3 f4 f5 f6 f7 f8 f9]轉置 = 0 8個點對 得到八個約束 A *f = 0 求 f 奇異值分解得到F 基礎矩陣 且其秩為2 需要再奇異值分解 後 取對角矩陣 秩為2 後在合成F * 【9】 基礎矩陣 F 求變換誤差 * * p2 ------> p1 * f11 f12 f13 u1 * (u2 v2 1) * f21 f22 f23 * v1 = 0 應該=0 不等於零的就是誤差 * f31 f32 f33 1 * a1 = f11*u2+f21*v2+f31; b1 = f12*u2+f22*v2+f32; c1 = f13*u2+f23*v2+f33; num1 = a1*u1 + b1*v1+ c1;// 應該等0 num1*num1/(a1*a1+b1*b1);// 誤差 * 【10】 從基本矩陣恢復 旋轉矩陣R 和 平移向量t 計算 本質矩陣 E = K轉置 * F * K 從本質矩陣恢復 旋轉矩陣R 和 平移向量t 恢復四種假設 並驗證 【11】2D-2D點三角化 得到對應的 三維點座標 平面二維點攝影矩陣到三維點 P1 = K × [I 0] P2 = K * [R t] kp1 = P1 * p3dC1 p3dC1 特徵點匹配對 對應的 世界3維點 kp2 = P2 * p3dC1 kp1 叉乘 P1 * p3dC1 =0 kp2 叉乘 P2 * p3dC1 =0 p = ( x,y,1) 其叉乘矩陣為 // 叉乘矩陣 = [0 -1 y; // 1 0 -x; // -y x 0 ] 一個方程得到兩個約束 對於第一行 0 -1 y; 會與P的三行分別相乘 得到四個值 與齊次3d點座標相乘得到 0 有 (y * P.row(2) - P.row(1) ) * D =0 (-x *P.row(2) + P.row(0) ) * D =0 ===> (x *P.row(2) - P.row(0) ) * D =0 兩個方程得到 4個約束 A × D = 0 對A進行奇異值分解 求解線性方程 得到 D （D是3維齊次座標，需要除以第四個尺度因子 歸一化） 2也可轉化到 相機歸一化平面下的點 x1 x2 p1 = k × [R1 t1] × D k逆 × p1 = [R1 t1] × D x1 = T1 × D x1叉乘x1 = x1叉乘T1 × D = 0 p2 = k × [ R2 t2] × D k逆 × p2 = [R2 t2] × D x2 = T2 × D x2叉乘x2 = x2叉乘T2 × D = 0 */ #include "Initializer.h" #include "Thirdparty/DBoW2/DUtils/Random.h" #include "Optimizer.h" #include "ORBmatcher.h" #include<thread> namespace ORB_SLAM2 { /** * @brief 類建構函式 給定參考幀構造Initializer 單目相機初始化參考幀 * * 用reference frame來初始化，這個reference frame就是SLAM正式開始的第一幀 * @param ReferenceFrame 參考幀 * @param sigma 測量誤差 * @param iterations RANSAC迭代次數 */ Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations) { mK = ReferenceFrame.mK.clone();// 相機內參數 mvKeys1 = ReferenceFrame.mvKeysUn;// 畸變校正後的 關鍵 點 mSigma = sigma;// 標準差 mSigma2 = sigma*sigma;// 方差 mMaxIterations = iterations;// 隨機取樣序列 最大迭代次數 } /** * @brief 類初始化函式 * 並行地計算基礎矩陣和單應性矩陣，選取其中一個模型，恢復出最開始兩幀之間的相對姿態以及點雲 * @param CurrentFrame 當前幀 和 第一幀 參考幀 匹配 三角變換得到 3D點 * @param vMatches12 當前幀 特徵點的匹配資訊 * @param R21 旋轉矩陣 * @param t21 平移矩陣 * @param vP3D 恢復出的3D點 * @param vbTriangulated 符合三角變換 的 3D點 */ 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 // Frame2 當前幀 畸變校正後的 關鍵 點 mvKeys2 = CurrentFrame.mvKeysUn;// 當前幀(2) 關鍵點 mvMatches12.clear();// 當前幀(2) 關鍵點 的匹配資訊 mvMatches12.reserve(mvKeys2.size()); // mvbMatched1記錄每個特徵點是否有匹配的特徵點， // 這個變數後面沒有用到，後面只關心匹配上的特徵點 mvbMatched1.resize(mvKeys1.size());// 匹配參考幀(1)關鍵點的匹配資訊 // 步驟1： 組織特徵點對 for(size_t i=0, iend=vMatches12.size();i<iend; i++) { if(vMatches12[i]>=0)// 幀2特徵點 有匹配 { mvMatches12.push_back(make_pair(i,vMatches12[i])); mvbMatched1[i]=true; } else mvbMatched1[i]=false;// 未匹配到 } // 匹配上的特徵點的個數 const int N = mvMatches12.size();// 有效的 匹配點對 個數 // 新建一個容器vAllIndices，生成0到N-1的數作為特徵點的索引 vector<size_t> vAllIndices; vAllIndices.reserve(N); vector<size_t> vAvailableIndices; for(int i=0; i<N; i++) { vAllIndices.push_back(i); } // Generate sets of 8 points for each RANSAC iteration // 步驟2： 在所有匹配特徵點對中隨機選擇8對匹配特徵點為一組，共選擇mMaxIterations組 // 用於FindHomography和FindFundamental求解 // mMaxIterations:200 // 隨機取樣序列 最大迭代次數 隨機序列 8點法 mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0)); DUtils::Random::SeedRandOnce(0);//隨機數 for(int it=0; it<mMaxIterations; it++) { //隨機引數 候選 點對 vAvailableIndices = vAllIndices;//候選匹配點對 // Select a minimum set for(size_t j=0; j<8; j++) { // 產生0到N-1的隨機數 int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);// 隨機數 // idx 表示哪一個索引對應的特徵點被選中 int idx = vAvailableIndices[randi];// 對應的隨機數 mvSets[it][j] = idx;//候選 匹配點對 // randi對應的索引已經被選過了，從容器中刪除 // randi對應的索引用最後一個元素替換，並刪掉最後一個元素 vAvailableIndices[randi] = vAvailableIndices.back(); vAvailableIndices.pop_back(); } } // 步驟3：呼叫多執行緒分別用於計算fundamental matrix和homography // Launch threads to compute in parallel a fundamental matrix and a homography // 啟動兩個執行緒 分別計算 基本矩陣 F 和 單應矩陣 vector<bool> vbMatchesInliersH, vbMatchesInliersF;//內點標誌 匹配點對是否在 計算出的 變換矩陣的 有效對映上 float SH, SF;// 最優變換矩陣 對應的 得分 cv::Mat H, F;//隨機取樣中計算得到 的 最優單應矩陣 H 和 基本矩陣 F // 計算 單應矩陣 homograpy 並打分 thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H)); // 計算 基礎矩陣 fundamental matrix並打分 thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F)); // Wait until both threads have finished // 等待兩個執行緒結束 threadH.join(); threadF.join(); // 步驟4：計算得分比例，選取某個模型 // 從兩個模型 H F 得分為 Sh Sf 中選著一個 最優秀的 模型 的方法為 // Compute ratio of scores float RH = SH/(SH+SF);// 計算 選著標誌 // 步驟5：從單應矩陣H 或 基礎矩陣F中恢復R,t // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45) if(RH>0.40)// 更偏向於 平面 使用 單應矩陣恢復 return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50); else //if(pF_HF>0.6) // 偏向於非平面 使用 基礎矩陣 恢復 return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50); return false; } /** * @brief 計算單應矩陣 隨機取樣序列 8點 採用歸一化的直接線性變換（normalized DLT） * 假設場景為平面情況下通過前兩幀求取Homography矩陣(current frame 2 到 reference frame 1) * 在最大迭代次數內 呼叫 ComputeH21 計算 使用 CheckHomography 計算單應 得分 * 並得到該模型的評分 * 在最大迭代次數內 保留 最高得分的 單應矩陣 * @param vbMatchesInliers 返回的 符合 變換的 匹配點 內點 標誌 * @param score 變換得分 * @param H21 單應矩陣 */ void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21) { // Number of putative matches const int N = mvMatches12.size();// 2中匹配的1中的點對 匹配點對總數 // 步驟1： // 將mvKeys1和mvKey2歸一化到均值為0，一階絕對矩為1，歸一化矩陣分別為T1、T2 //2D-2D點對 求變換矩陣前先進行標準化 去均值點座標 * 絕對矩倒數 //標準化矩陣 * 點座標 = 標準化後的的座標 // 點座標 = 標準化矩陣 逆矩陣 * 標準化後的的座標 // Normalize coordinates vector<cv::Point2f> vPn1, vPn2;// 2d-2d點對 cv::Mat T1, T2;// 標準化矩陣 Normalize(mvKeys1,vPn1, T1);// 標準化點座標 去均值點座標 * 絕對矩倒數 Normalize(mvKeys2,vPn2, T2);// cv::Mat T2inv = T2.inv();// 標準化矩陣 逆矩陣 // Best Results variables // 最終最佳的MatchesInliers與得分 score = 0.0; vbMatchesInliers = vector<bool>(N,false);// 內點 標誌 // Iteration variables vector<cv::Point2f> vPn1i(8);// 隨機 取樣 8點對 vector<cv::Point2f> vPn2i(8); cv::Mat H21i, H12i;// 原點對 的 單應矩陣 // H21i 原始點 p1 ----------------> p2 的單應 vector<bool> vbCurrentInliers(N,false);//當前隨機點裡的 內點 float currentScore; // 步驟2：隨機取樣序列迭代求解 // Perform all RANSAC iterations and save the solution with highest score for(int it=0; it<mMaxIterations; it++)//在最大迭代次數內 { // Select a minimum set //步驟3：隨機8對點對 for(size_t j=0; j<8; j++) { int idx = mvSets[it][j];//隨機數集合 總匹配點數範圍內 vPn1i[j] = vPn1[mvMatches12[idx].first]; vPn2i[j] = vPn2[mvMatches12[idx].second]; } // Hn T2逆*Hn*T1 // 步驟4：計算 單應矩陣 T1*p1 ----> T2*p2 p1 ----------------> p2 cv::Mat Hn = ComputeH21(vPn1i,vPn2i);// 計算標準化後的點對 的 單應矩陣 H21i = T2inv*Hn*T1;// 原始點 p1 ----------------> p2 的單應 H12i = H21i.inv();// 原始點 p2 ----------------> p1 的單應 // 計算單應 轉換矩陣得分 /* * 變換矩陣 評分 方法 * SM=∑i( ρM( d2cr(xic,xir,M) + ρM( d2rc(xic,xir,M ) ) * d2cr 為 2D-2D 點對 通過哦轉換矩陣的 對成轉換誤差 * * ρM 函式為 ρM(d^2) = 0 當 d^2 > 閾值(單應矩陣時 為 5.99 基礎矩陣時為 3.84) * 最高分 - d^2 當 d^2 < 閾值 * 最高分 均為 5.99 */ // 步驟5：計算單應H的得分 有由 對應的匹配點對 的 對稱的轉換誤差 求得 currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); // 步驟6：保留最高得分 對應的 單應 if(currentScore > score)//此次迭代 計算的單應H的得分較高 { H21 = H21i.clone();//保留較高得分的單應 vbMatchesInliers = vbCurrentInliers;//對應的匹配點對 score = currentScore;// 最高的得分 } } } // 計算基礎矩陣 隨機取樣序列 8點 採用歸一化的直接線性變換（normalized DLT） // 在最大迭代次數內 呼叫 ComputeH21 計算 使用 CheckHomography 計算單應 得分 // 在最大迭代次數內 保留 最高得分的 單應矩陣 /** * @brief 計算基礎矩陣 * * 假設場景為非平面情況下通過前兩幀求取Fundamental矩陣(current frame 2 到 reference frame 1),並得到該模型的評分 */ void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21) { // Number of putative matches // 總匹配點數 const int N = vbMatchesInliers.size(); /* *【1】2D-2D點對 求變換矩陣前先進行標準化 去均值點座標 * 絕對矩倒數 * 標準化矩陣 * 點座標 = 標準化後的的座標 * 點座標 = 標準化矩陣 逆矩陣 * 標準化後的的座標 */ // Normalize coordinates vector<cv::Point2f> vPn1, vPn2;// 標準化後的的座標 cv::Mat T1, T2; Normalize(mvKeys1,vPn1, T1);// 標準化 去均值點座標 * 絕對矩倒數 Normalize(mvKeys2,vPn2, T2); cv::Mat T2t = T2.t();// 標準化矩陣 逆矩陣 // Best Results variables score = 0.0; vbMatchesInliers = vector<bool>(N,false);// 最優 基本矩陣變換 對應 的點對的標記 1是 內點 0 是野點 // Iteration variables // 隨機8對 點對 vector<cv::Point2f> vPn1i(8); vector<cv::Point2f> vPn2i(8); cv::Mat F21i; vector<bool> vbCurrentInliers(N,false);//每次迭代 求解的 點對的標記 1是 內點 0 是野點 float currentScore; // 【2】隨機取樣序列迭代求解 // Perform all RANSAC iterations and save the solution with highest score for(int it=0; it<mMaxIterations; it++) { // Select a minimum set //【3】隨機8對點對 for(int j=0; j<8; j++) { int idx = mvSets[it][j]; vPn1i[j] = vPn1[mvMatches12[idx].first]; vPn2i[j] = vPn2[mvMatches12[idx].second]; } // Fn T2逆*Fn*T1 // 【4】計算 基礎矩陣 T1*p1 ----> T2*p2 p1 ----------------> p2 cv::Mat Fn = ComputeF21(vPn1i,vPn2i); F21i = T2t*Fn*T1; // 計算基礎矩陣 F轉換矩陣得分 /* * 變換矩陣 評分 方法 * SM=∑i( ρM( d2cr(xic,xir,M) + ρM( d2rc(xic,xir,M ) ) * d2cr 為 2D-2D 點對 通過哦轉換矩陣的 對成轉換誤差 * * ρM 函式為 ρM(d^2) = 0 當 d^2 > 閾值(單應矩陣時 為 5.99 基礎矩陣時為 3.84) * 最高分 - d^2 當 d^2 < 閾值 * 最高分 均為 5.99 */ // 【5】計算基礎矩陣 F的得分 有由 對應的匹配點對 的 對稱的轉換誤差 求得 currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma); // 【6】保留最高得分 對應的 基礎矩陣 F if(currentScore>score) { F21 = F21i.clone();// 最優的 基礎矩陣 F vbMatchesInliers = vbCurrentInliers;//保持最優的 每次迭代 求解的 點對的標記 1是 內點 0 是野點 score = currentScore;//當前得分 } } } // 計算單應矩陣 8對點對 每個點提供兩個約束 A × h = 0 求h 奇異值分解 求 h // // 通過svd進行最小二乘求解 // 參考 http://www.fengbing.net/ // |x'| | h1 h2 h3 ||x| // |y'| = a | h4 h5 h6 ||y| 簡寫: x' = a H x, a為一個尺度因子 // |1 | | h7 h8 h9 ||1| // 使用DLT(direct linear tranform)求解該模型 // x' = a H x // ---> (x') 叉乘 (H x) = 0 // ---> 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'A最小特徵值對應的特徵向量即為解 /** * @brief 從特徵點匹配求homography（normalized DLT） * * @param vP1 歸一化後的點, in reference frame * @param vP2 歸一化後的點, in current frame * @return 單應矩陣 * @see Multiple View Geometry in Computer Vision - Algorithm 4.2 p109 */ cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) { const int N = vP1.size();// 8 點對 cv::Mat A(2*N,9,CV_32F);// 每個點 可以提供兩個約束 單應為 3*3 9個 元素 /* * 1點 變成 2點 p2 = H21 * p1 u2 h1 h2 h3 u1 v2 = h4 h5 h6 * v1 1 h7 h8 h9 1 或是使用叉乘 得到0 * x = H y ，則對向量 x和Hy 進行叉乘為0，即： * | 0 -1 v2| |h1 h2 h3| |u1| |0| * | 1 0 -u2| * |h4 h5 h6| * |v1| = |0| * |-v2 u2 0| |h7 h8 h9| |1 | |0| u2 = (h1*u1 + h2*v1 + h3) /( h7*u1 + h8*v1 + h9) v2 = (h4*u1 + h5*v1 + h6) /( h7*u1 + h8*v1 + h9) -((h4*u1 + h5*v1 + h6) - ( h7*u1*v2 + h8*v1*v2 + h9*v2))=0 式子為0 左側加 - 號不變 h1*u1 + h2*v1 + h3 - ( h7*u1*u2 + h8*v1*u2 + h9*u2)=0 0 0 0 -u1 -v1 -1 u1*v2 v1*v2 v2 u1 v1 1 0 0 0 -u1*u2 - v1*u2 -u2 ×(h1 h2 h3 h4 h5 h6 h7 h8 h9)轉置 = 0 8對點 約束 A A × h = 0 求h 奇異值分解 A 得到 單元矩陣 H */ for(int i=0; i<N; i++)//8對點 { 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>(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; 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; } cv::Mat u,w,vt; // A × h = 0 求h // 在matlab中，[U,S,V]=svd(A)，其中U和V代表二個相互正交矩陣，而S代表一對角矩陣。 //和QR分解法相同者， 原矩陣A不必為正方矩陣。 //使用SVD分解法的用途是解最小平方誤差法和資料壓縮。 // cv::SVDecomp(A,S,U,VT,SVD::FULL_UV); //後面的FULL_UV表示把U和VT補充稱單位正交方陣; cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 奇異值分解 return vt.row(8).reshape(0, 3);// v的最後一列 } // 通過svd進行最小二乘求解 // 8 對點 每個點 提供 一個約束 //8個點對 得到八個約束 //A *f = 0 求 f 奇異值分解 得到 f /** * 構建基礎矩陣的約束方程，給定一對點對應m=(u1,v1,1)T, m'=(u2,v2,1)T * 滿足基礎矩陣F m'T F m=0,令F=(f_ij),則約束方程可以化簡為： * u2u1 f_11 + u2v1 f_12 + u2 f_13+v2u1f_21+v2v1f_22+v2f_23+u1f_31+v1f_32+f_33=0 * 令f = (f_11,f_12,f_13,f_21,f_22,f_23,f_31,f_32,f_33) * 則(u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1)f=0; * 這樣，給定N個對應點就可以得到線性方程組Af=0 * A就是一個N*9的矩陣，由於基礎矩陣是非零的，所以f是一個非零向量，即 * 線性方程組有非零解，另外基礎矩陣的秩為2，重要的約束條件 */ // 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最小特徵值對應的特徵向量即為解 /** * @brief 從特徵點匹配求fundamental matrix（normalized 8點法） * @param vP1 歸一化後的點, in reference frame * @param vP2 歸一化後的點, in current frame * @return 基礎矩陣 * @see Multiple View Geometry in Computer Vision - Algorithm 11.1 p282 (中文版 p191) */ cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2) { const int N = vP1.size(); cv::Mat A(N,9,CV_32F); /* * * p2------> p1 * f1 f2 f3 u1 * (u2 v2 1) * f4 f5 f6 * v1 = 0 應該=0 不等於零的就是誤差 * f7 f8 f9 1 * a1 = f1*u2 + f4*v2 + f7; b1 = f2*u2 + f5*v2 + f8; c1 = f3*u2 + f6*v2 + f9; a1*u1+ b1*v1+ c1= 0 一個點對 得到一個約束方程 f1*u1*u2 + f2*v1*u2 + f3*u2 + f4*u1*v2 + f5*v1*v2 + f6*v2 + f7*u1 + f8*v1 + f9 =0 [ u1*u2 v1*u2 u2 u1*v2 v1*v2 v2 u1 v1 1 ] * [f1 f2 f3 f4 f5 f6 f7 f8 f9]轉置 = 0 8個點對 得到八個約束 A *f = 0 求 f 奇異值分解得到F 基礎矩陣 且其秩為2 需要再奇異值分解 後 取對角矩陣 秩為2 在合成F */ 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; cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV); cv::Mat Fpre = vt.row(8).reshape(0, 3);// F 基礎矩陣的秩為2 需要在分解 後 取對角矩陣 秩為2 在合成F cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV); w.at<float>(2)=0;// 基礎矩陣的秩為2，重要的約束條件 return u * cv::Mat::diag(w) * vt;// 在合成F } // 計算單應矩陣 得分 /* * 【3】變換矩陣 評分 方法 * SM=∑i( ρM( d2cr(xic,xir,M) + ρM( d2rc(xic,xir,M ) ) * d2cr 為 2D-2D 點對 通過哦轉換矩陣的 對成轉換誤差 * * ρM 函式為 ρM(d^2) = 0 當 d^2 > 閾值(單應矩陣時 為 5.991 基礎矩陣時為 3.84) * 閾值 - d^2 當 d^2 < 閾值 * */ /** * @brief 對給定的homography matrix打分 * * @see * - Author's paper - IV. AUTOMATIC MAP INITIALIZATION （2） * - Multiple View Geometry in Computer Vision - symmetric transfer errors: 4.2.2 Geometric distance * - Multiple View Geometry in Computer Vision - model selection 4.7.1 RANSAC */ float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma) { const int N = mvMatches12.size();// 總匹配點對數量 // |h11 h12 h13| // |h21 h22 h23| // |h31 h32 h33| const float h11 = H21.at<float>(0,0); // p1 ----> p2 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); // |h11inv h12inv h13inv| // |h21inv h22inv h23inv| // |h31inv h32inv h33inv| 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); vbMatchesInliers.resize(N);// 匹配點對是否在 變換矩陣對於的 變換上 是否是內點 float score = 0; // 基於卡方檢驗計算出的閾值（假設測量有一個畫素的偏差） const float th = 5.991;// 單應變換誤差 閾值 //資訊矩陣，方差平方的倒數 const float invSigmaSquare = 1.0/(sigma*sigma);//方差 倒數 // N對特徵匹配點 for(int i=0; i<N; i++)//計算單應矩陣 變換 每個點對時產生 的 對稱的轉換誤差 { bool bIn = true; // 關鍵點座標 const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first]; const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second]; /* * * 1點 變成 2點 u2 h11 h12 h13 u1 v2 = h21 h22 h23 * v1 1 h31 h32 h33 1 第三行 * 2 點 變成 1點 u1‘ h11inv h12inv h13inv u2 v1’ = h21inv h22inv h23inv * v2 1 h31inv h32inv h33inv 1 第三行 h31inv*u2+h32inv*v2+h33inv 前兩行 同除以 第三行 消去非零因子 p2 由單應轉換到 p1 u1‘ = (h11inv*u2+h12inv*v2+h13inv)* 第三行倒數 v1’ = (h21inv*u2+h22inv*v2+h23inv)*第三行倒數 然後計算 和 真實 p1點 座標的差值 (u1-u2in1)*(u1-u2in1) + (v1-v2in1)*(v1-v2in1) 橫縱座標差值平方和 */ // 步驟1： p2 由單應轉換到 p1 距離誤差 以及得分 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 first image // x2in1 = H12*x2 // 將影象2中的特徵點單應到影象1中 // |u1| |h11inv h12inv h13inv||u2| // |v1| = |h21inv h22inv h23inv||v2| // |1 | |h31inv h32inv h33inv||1 | const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);//第三行倒數 const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;// p2 由單應轉換到 p1‘ const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv; const float squareDist1 = (u1-u2in1)*(u1-u2in1) + (v1-v2in1)*(v1-v2in1);// 橫縱座標差值平方和 const float chiSquare1 = squareDist1*invSigmaSquare;// 根據方差歸一化誤差 if(chiSquare1>th)//距離大於閾值 改點 變換的效果差 bIn = false; else score += th - chiSquare1;// 閾值 - 距離差值 得到 得分，差值越小 得分越高 // 步驟2：p1由單應轉換到 p2 距離誤差 以及得分 // Reprojection error in second image // x1in2 = H21*x1 p1點 變成p2點 誤差 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; // 計算重投影誤差 const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);// 計算重投影誤差 // 根據方差歸一化誤差 const float chiSquare2 = squareDist2*invSigmaSquare; if(chiSquare2>th) bIn = false; else score += th - chiSquare2; if(bIn) vbMatchesInliers[i]=true;// 是內點 誤差較小 else vbMatchesInliers[i]=false;// 是野點 誤差較大 } return score; } // 計算 基礎矩陣 得分 // 和卡方分佈的對應值比較，由此判定該點是否為內點。累計內點的總得分 // p2轉置 * F * p1 = 0 /* * p2 ------> p1 * f11 f12 f13 u1 * (u2 v2 1) * f21 f22 f23 * v1 = 0應該=0 不等於零的就是誤差 * f31 f32 f33 1 * a1 = f11*u2+f21*v2+f31; b1 = f12*u2+f22*v2+f32; c1 = f13*u2+f23*v2+f33; num1 = a1*u1 + b1*v1+ c1;// 應該等0 num1*num1/(a1*a1+b1*b1);// 誤差 */ /** * @brief 對給定的fundamental matrix打分 * p2 轉置 * F21 * p1 = 0 * F21 * p1為 幀1 關鍵點 p1在 幀2 上的極線 l1 * * * p2 應該在這條極限附近 求p2 到極線 l的距離 可以作為誤差 * 極線l：ax + by + c = 0 * (u,v)到l的距離為：d = |au+bv+c| / sqrt(a^2+b^2) * d^2 = |au+bv+c|^2/(a^2+b^2) * * p2 轉置 * F21 為幀2 關鍵點 p2在 幀1 上的極線 l2 * * * @see * - Author's paper - IV. AUTOMATIC MAP INITIALIZATION （2） * - Multiple View Geometry in Computer Vision - symmetric transfer errors: 4.2.2 Geometric distance * - Multiple View Geometry in Computer Vision - model selection 4.7.1 RANSAC */ float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma) { const int N = mvMatches12.size(); 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); vbMatchesInliers.resize(N); float score = 0; // 基於卡方檢驗計算出的閾值（假設測量有一個畫素的偏差） const float th = 3.841; const float thScore = 5.991; //資訊矩陣，方差平方的倒數 const float invSigmaSquare = 1.0/(sigma*sigma); for(int i=0; i<N; i++) { bool bIn = true; 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; // p1 ------> p2 誤差 得分------------------------------- // Reprojection error in second image // l2=F21 x1=(a2,b2,c2) // F21x1可以算出x1在影象中x2對應的線l const float a2 = f11*u1+f12*v1+f13; const float b2 = f21*u1+f22*v1+f23; const float c2 = f31*u1+f32*v1+f33; // x2應該在l這條線上:x2點乘l = 0 // 計算x2特徵點到 極線 的距離： // 極線l：ax + by + c = 0 // (u,v)到l的距離為：d = |au+bv+c| / sqrt(a^2+b^2) // d^2 = |au+bv+c|^2/(a^2+b^2) const float num2 = a2*u2+b2*v2+c2; const float squareDist1 = num2*num2/(a2*a2+b2*b2);// 點到線的幾何距離 的平方 // 根據方差歸一化誤差 const float chiSquare1 = squareDist1*invSigmaSquare; if(chiSquare1>th) bIn = false; else score += thScore - chiSquare1;//得分 // Reprojection error in second image // l1 =x2轉置 × F21=(a1,b1,c1) // p2 ------> p1 誤差 得分------------------------- const float a1 = f11*u2+f21*v2+f31; const float b1 = f12*u2+f22*v2+f32; const float c1 = f13*u2+f23*v2+f33; const float num1 = a1*u1+b1*v1+c1; const float squareDist2 = num1*num1/(a1*a1+b1*b1); const float chiSquare2 = squareDist2*invSigmaSquare; if(chiSquare2>th) bIn = false; else score += thScore - chiSquare2;// 得分 if(bIn) vbMatchesInliers[i]=true;//內點 誤差較小 else vbMatchesInliers[i]=false;// 野點 誤差較大 } return score; } /* 從基本矩陣恢復 旋轉矩陣R 和 平移向量t 計算 本質矩陣 E = K轉置逆 * F * K 從本質矩陣恢復 旋轉矩陣R 和 平移向量t 恢復四種假設 並驗證 理論參考 Result 9.19 in Multiple View Geometry in Computer Vision */ // |0 -1 0| // E = U Sigma V' let W = |1 0 0| 為RZ(90) 繞Z軸旋轉 90度（x變成原來的y y變成原來的-x z軸沒變） // |0 0 1| // 得到4個解 E = [R|t] // R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3 /** * @brief 從基本矩陣 F 恢復R t * * 度量重構 * 1. 由Fundamental矩陣結合相機內參K，得到Essential矩陣: \f$ E = k轉置F k \f$ * 2. SVD分解得到R t * 3. 進行cheirality check, 從四個解中找出最合適的解 * * @see Multiple View Geometry in Computer Vision - Result 9.19 p259 */ 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) { int N=0; for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++) if(vbMatchesInliers[i])// 是內點 N++;// 符合 基本矩陣 F的 內點數量 // Compute Essential Matrix from Fundamental Matrix // 步驟1： 計算 本質矩陣 E = K轉置 * F * K cv::Mat E21 = K.t()*F21*K; cv::Mat R1, R2, t; // Recover the 4 motion hypotheses 四種運動假設 /* // 步驟2： 從本質矩陣恢復 旋轉矩陣R 和 平移向量t * 對 本質矩陣E 進行奇異值分解 得到可能的解 * t = u * RZ(90) * u轉置 * R= u * RZ(90) * V轉置 * 組合情況有四種 */ // 雖然這個函式對t有歸一化，但並沒有決定單目整個SLAM過程的尺度 // 因為CreateInitialMapMonocular函式對3D點深度會縮放，然後反過來對 t 有改變 DecomposeE(E21,R1,R2,t); cv::Mat t1=t; cv::Mat t2=-t; // 步驟3： 恢復四種假設 並驗證 Reconstruct with the 4 hyphoteses and check // 這4個解中只有一個是合理的，可以使用視覺化約束來選擇， // 與單應性矩陣做sfm一樣的方法，即將4種解都進行三角化，然後從中選擇出最合適的解。 vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4; vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4; float parallax1,parallax2, parallax3, parallax4; int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,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); int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4))); R21 = cv::Mat(); t21 = cv::Mat(); // minTriangulated為可以三角化恢復三維點的個數 int nMinGood = max(static_cast<int>(0.9*N),minTriangulated); int 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++; // If there is not a clear winner or not enough triangulated points reject initialization if(maxGood<nMinGood || nsimilar>1) {// 四個結果中如果沒有明顯的最優結果，則返回失敗 return false;// 初始化失敗 } // If best reconstruction has enough parallax initialize // 比較大的視差角 四種假設 if(maxGood==nGood1) { if(parallax1>minParallax) { vP3D = vP3D1; vbTriangulated = vbTriangulated1; R1.copyTo(R21); t1.copyTo(t21); 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;// 初始化成功 } } return false;// 初始化失敗 } /* 從單應矩陣恢復 旋轉矩陣R 和 平移向量t 理論參考 // Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988. https://hal.archives-ouvertes.fr/inria-00075698/document p2 = H21 * p1 p2 = K( RP + t) = KTP = H21 * KP T = K 逆 * H21*K 在求得單應性變化H後，本文使用FAUGERAS的論文[1]的方法，提取8種運動假設。 這個方法通過視覺化約束來測試選擇合理的解。但是如果在低視差的情況下， 點雲會跑到相機的前面或後面，測試就會出現錯誤從而選擇一個錯誤的解。 文中使用的是直接三角化 8種方案，檢查兩個相機前面具有較少的重投影誤差情況下， 在檢視低視差情況下是否大部分雲點都可以看到。如果沒有一個解很合適，就不執行初始化， 重新從第一步開始。這種方法在低視差和兩個交叉的檢視情況下，初始化程式更具魯棒性。 */ // H矩陣分解常見有兩種方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition // 參考文獻：Motion and structure from motion in a piecewise plannar environment // 這篇參考文獻和下面的程式碼使用了Faugeras SVD-based decomposition演算法 /** * @brief 從H恢復R t * * @see * - Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988. * - Deeper understanding of the homography decomposition for vision-based control */ 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) { int N=0; for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++) if(vbMatchesInliers[i]) N++;//匹配點對 內點 // 8種運動假設 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 // 因為特徵點是影象座標系，所以將H矩陣由相機座標系換算到影象座標系 cv::Mat invK = K.inv(); cv::Mat A = invK*H21*K; cv::Mat U,w,Vt,V; cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV); V=Vt.t(); float s = cv::determinant(U)*cv::determinant(Vt); float d1 = w.at<float>(0); float d2 = w.at<float>(1); float d3 = w.at<float>(2); // SVD分解的正常情況是特徵值降序排列 if(d1/d2<1.00001 || d2/d3<1.00001) { return false;// 初始化失敗 } vector<cv::Mat> vR, vt, vn; vR.reserve(8); vt.reserve(8); vn.reserve(8); //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1 // 法向量n'= [x1 0 x3] 對應ppt的公式17 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}; //case d'=d2 // 計算ppt中公式19 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}; // 計算旋轉矩陣 R‘，計算ppt中公式18 // | 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++) { 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; cv::Mat R = s*U*Rp*Vt; vR.push_back(R); 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; // 這裡雖然對t有歸一化，並沒有決定單目整個SLAM過程的尺度 // 因為CreateInitialMapMonocular函式對3D點深度會縮放，然後反過來對 t 有改變 cv::Mat t = U*tp; vt.push_back(t/cv::norm(t)); cv::Mat np(3,1,CV_32F); np.at<float>(0)=x1[i]; np.at<float>(1)=0; np.at<float>(2)=x3[i]; cv::Mat n = V*np; if(n.at<float>(2)<0) n=-n; vn.push_back(n); } //case d'=-d2 // 計算ppt中公式22 float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2); float cphi = (d1*d3-d2*d2)/((d1-d3)*d2); float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi}; // 計算旋轉矩陣 R‘，計算ppt中公式21 for(int i=0; i<4; i++) { 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; cv::Mat R = s*U*Rp*Vt; vR.push_back(R); 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; cv::Mat t = U*tp; vt.push_back(t/cv::norm(t)); cv::Mat np(3,1,CV_32F); np.at<float>(0)=x1[i]; np.at<float>(1)=0; np.at<float>(2)=x3[i]; cv::Mat n = V*np; if(n.at<float>(2)<0) n=-n; vn.push_back(n); } int bestGood = 0; int secondBestGood = 0; int bestSolutionIdx = -1; float bestParallax = -1; vector<cv::Point3f> bestP3D; vector<bool> bestTriangulated; // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax) // We reconstruct all hypotheses and check in terms of triangulated points and parallax // d'=d2和d'=-d2分別對應8組(R t) for(size_t i=0; i<8; i++) { float parallaxi; vector<cv::Point3f> vP3Di; vector<bool> vbTriangulatedi; int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi); // 保留最優的和次優的 if(nGood>bestGood) { secondBestGood = bestGood; bestGood = nGood; bestSolutionIdx = i; bestParallax = parallaxi; bestP3D = vP3Di; bestTriangulated = vbTriangulatedi; } else if(nGood>secondBestGood) { secondBestGood = nGood; } } if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N) { vR[bestSolutionIdx].copyTo(R21); vt[bestSolutionIdx].copyTo(t21); vP3D = bestP3D; vbTriangulated = bestTriangulated; return true;// 初始化成功 } return false;// 初始化失敗 } /* * 三角化得到3D點 * *三角測量法 求解 兩組單目相機 影象點深度 * s1 * x1 = s2 * R * x2 + t * x1 x2 為兩幀影象上 兩點對 在歸一化座標平面上的座標 k逆* p * s1 和 s2為兩個特徵點的深度 ，由於誤差存在， s1 * x1 = s2 * R * x2 + t不精確相等 * 常見的是求解最小二乘解，而不是零解 * s1 * x1叉乘x1 = s2 * x1叉乘* R * x2 + x1叉乘 t=0 可以求得x2 * */ /* 平面二維點攝影矩陣到三維點 P1 = K × [I 0] P2 = K * [R t] kp1 = P1 * p3dC1 p3dC1 特徵點匹配對 對應的 世界3維點 kp2 = P2 * p3dC1 kp1 叉乘 P1 * p3dC1 =0 kp2 叉乘 P2 * p3dC1 =0 p = ( x,y,1) 其叉乘矩陣為 // 叉乘矩陣 = [0 -1 y; // 1 0 -x; // -y x 0 ] 一個方程得到兩個約束 對於第一行 0 -1 y; 會與P的三行分別相乘 得到四個值 與齊次3d點座標相乘得到 0 有 (y * P.row(2) - P.row(1) ) * D =0 (-x *P.row(2) + P.row(0) ) * D =0 ===> (x *P.row(2) - P.row(0) ) * D =0 兩個方程得到 4個約束 A × D = 0 對A進行奇異值分解 求解線性方程 得到 D （D是3維齊次座標，需要除以第四個尺度因子 歸一化） */ // Trianularization: 已知匹配特徵點對{x x'} 和 各自相機矩陣{P P'}, 估計三維點 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| // 兩個點: // |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| /** * @brief 給定投影矩陣P1,P2和影象上的點kp1,kp2，從而恢復3D座標 * * @param kp1 特徵點, in reference frame * @param kp2 特徵點, in current frame * @param P1 投影矩陣P1 * @param P2 投影矩陣P２ * @param x3D 三維點 * @see Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312 */ void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) { // 在DecomposeE函式和ReconstructH函式中對t有歸一化 // 這裡三角化過程中恢復的3D點深度取決於 t 的尺度， // 但是這裡恢復的3D點並沒有決定單目整個SLAM過程的尺度 // 因為CreateInitialMapMonocular函式對3D點深度會縮放，然後反過來對 t 有改變 cv::Mat A(4,4,CV_32F); A.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; cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV); x3D = vt.row(3).t(); x3D = x3D.rowRange(0,3)/x3D.at<float>(3);// 轉換成非齊次座標 歸一化 } // 對 2D 點對進行標準化 /* *【0】2D-2D點對 求變換矩陣前先進行標準化 去 均值後再除以絕對矩 * 單目初始化特徵點 歸一化 座標均值為0 一階絕對矩為1 * mean_x = sum( ui) / N mean_y = sum(vi)/N * 絕對矩 mean_x_dev = sum（abs(ui - mean_x)）/ N mean_y_dev = sum（abs(vi - mean_y)）/ N * *絕對矩倒數 sX = 1/mean_x_dev sY = 1/mean_y_dev * * 標準化後的點座標 * u = (ui - mean_x) × sX * v = (vi - mean_y) * sY * * 標準化矩陣 其逆矩陣×標準化點 得到原始座標 * 用於 計算變換矩陣後 從原始座標計算對稱的轉換誤差 來計算變換矩陣得分 * T = sX 0 -mean_x * sX * 0 sY -mean_y * sY * 0 0 1 * * 標準化矩陣 * 點座標 = 標準化後的的座標 * ui ui × sX - mean_x * sX = (ui - mean_x) × sX u * T × vi = vi × sY - mean_y * sY = (vi - mean_y) × sY = v * 1 1 1 * * * 點座標 = 標準化矩陣 逆矩陣 * 標準化後的的座標 * */ /** * ＠brief 歸一化特徵點到同一尺度（作為normalize DLT的輸入） * * [x' y' 1]' = T * [x y 1]' \n * 歸一化後x', y'的均值為0，sum(abs(x_i'-0))=1，sum(abs((y_i'-0))=1 * * @param vKeys 特徵點在影象上的座標 * @param vNormalizedPoints 特徵點歸一化後的座標 * @param T 將特徵點歸一化的矩陣 */ void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T) { const int N = vKeys.size();// 點總數 vNormalizedPoints.resize(N);//標準化後的點 float meanX = 0;//橫座標均值 float meanY = 0;//縱座標均值 for(int i=0; i<N; i++) { meanX += vKeys[i].pt.x;// 橫座標之和 meanY += vKeys[i].pt.y;//縱座標之和 } meanX = meanX/N;//橫座標均值 meanY = meanY/N;//縱座標均值 float meanDevX = 0;//絕對矩 float meanDevY = 0;//絕對矩 // 將所有vKeys點減去中心座標，使x座標和y座標均值分別為0 for(int i=0; i<N; i++) { vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;// 去均值點座標 vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;// meanDevX += fabs(vNormalizedPoints[i].x);// 總絕對矩 meanDevY += fabs(vNormalizedPoints[i].y); } meanDevX = meanDevX/N;//均值絕對矩 meanDevY = meanDevY/N; float sX = 1.0/meanDevX; float sY = 1.0/meanDevY; // 將x座標和y座標分別進行尺度縮放，使得x座標和y座標的一階絕對矩分別為1 for(int i=0; i<N; i++) { // 標註化後的點座標 vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;// 去均值點座標 * 絕對矩倒數 vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY; } // |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; } /* * 檢查求得的R t 是否符合 * 接受 R,t ，一組成功的匹配。最後給出的結果是這組匹配中有多少匹配是 * 能夠在這組 R,t 下正確三角化的（即 Z都大於0），並且輸出這些三角化之後的三維點。 如果三角化生成的三維點 Z小於等於0，且三角化的“前方交會角”（餘弦是 cosParallax）不會太小， 那麼這個三維點三角化錯誤，捨棄。 通過了 Z的檢驗，之後將這個三維點分別投影到兩張影像上， 計算投影的畫素誤差，誤差大於2倍中誤差，捨棄。 */ /** * @brief 進行cheirality check，從而進一步找出F分解後最合適的解 */ 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 ¶llax) { // Calibration parameters // 校正引數 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); vbGood = vector<bool>(vKeys1.size(),false); vP3D.resize(vKeys1.size());// 對應的三維點 vector<float> vCosParallax; vCosParallax.reserve(vKeys1.size()); // Camera 1 Projection Matrix K[I|0] // 步驟1：得到一個相機的投影矩陣 // 以第一個相機的光心作為世界座標系 // 相機1 變換矩陣 在第一幅影象下 的變換矩陣 Pc1 = Pw = T1 * Pw T1 = [I|0] // Pp1 = K * Pc1 = K * T1 * Pw = [K|0] *Pw = P1 × Pw cv::Mat P1(3,4,CV_32F,cv::Scalar(0)); K.copyTo(P1.rowRange(0,3).colRange(0,3)); // 第一個相機的光心在世界座標系下的座標 cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);// 相機1原點 000 // 步驟2：得到第二個相機的投影矩陣 // Camera 2 Projection Matrix K[R|t] // 相機2 變換矩陣 Pc2 = Pw = T2 * Pw T2 = [R|t] // Pp2 = K * Pc2 = K * T2 * Pw = K* [R|t] *Pw = P2 × Pw cv::Mat P2(3,4,CV_32F); R.copyTo(P2.rowRange(0,3).colRange(0,3)); t.copyTo(P2.rowRange(0,3).col(3)); P2 = K*P2; // 第二個相機的光心在世界座標系下的座標 cv::Mat O2 = -R.t()*t;//相機2原點 R逆 * - t R 為正交矩陣 逆 = 轉置 int nGood=0; for(size_t i=0, iend=vMatches12.size();i<iend;i++)// 每一個匹配點對 { if(!vbMatchesInliers[i])// 離線點 非內點 continue; // kp1和kp2是匹配特徵點 const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first]; const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second]; cv::Mat p3dC1; // 步驟3：利用三角法恢復三維點p3dC1 // kp1 = P1 * p3dC1 kp2 = P2 * p3dC1 Triangulate(kp1,kp2,P1,P2,p3dC1); if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2))) {// 求出的3d點座標 值有效 vbGood[vMatches12[i].first]=false; continue; } // 步驟4：計算視差角餘弦值 // Check parallax cv::Mat normal1 = p3dC1 - O1; float dist1 = cv::norm(normal1); cv::Mat normal2 = p3dC1 - O2; float dist2 = cv::norm(normal2); float cosParallax = normal1.dot(normal2)/(dist1*dist2); // 步驟5：判斷3D點是否在兩個攝像頭前方 // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth) // 步驟5.1：3D點深度為負，在第一個攝像頭後方，淘汰 if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998) continue; // 步驟5.2：3D點深度為負，在第二個攝像頭後方，淘汰 // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth) cv::Mat p3dC2 = R*p3dC1+t; if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998) continue; // 步驟6：計算重投影誤差 // Check reprojection error in first image // 計算3D點在第一個影象上的投影誤差 float im1x, im1y; 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); // 步驟6.1：重投影誤差太大，跳過淘汰 // 一般視差角比較小時重投影誤差比較大 if(squareError1>th2) continue; // 計算3D點在第二個影象上的投影誤差 // Check reprojection error in second image float im2x, im2y; 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); // 步驟6.2：重投影誤差太大，跳過淘汰 // 一般視差角比較小時重投影誤差比較大 if(squareError2>th2) continue; // 步驟7：統計經過檢驗的3D點個數，記錄3D點視差角 vCosParallax.push_back(cosParallax); vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2)); //nGood++; if(cosParallax<0.99998){ vbGood[vMatches12[i].first]=true; // WYW 20180130 修改 nGood++; } } // 步驟8：得到3D點中較大的視差角 if(nGood>0) { sort(vCosParallax.begin(),vCosParallax.end());// 從小到大排序 // trick! 排序後並沒有取最大的視差角 // 取一個較大的視差角 size_t idx = min(50,int(vCosParallax.size()-1)); parallax = acos(vCosParallax[idx])*180/CV_PI; } else parallax=0; return nGood; } /* * 從本質矩陣恢復 旋轉矩陣R 和 平移向量t * 對 本質矩陣E 進行奇異值分解 得到可能的解 * t = u * RZ(90) * u轉置 * R= u * RZ(90) * V轉置 * 組合情況有四種 */ /** * @brief 分解Essential矩陣 * * F矩陣通過結合內參可以得到Essential矩陣，分解E矩陣將得到4組解 \n * 這4組解分別為[R1,t],[R1,-t],[R2,t],[R2,-t] * @param E Essential Matrix * @param R1 Rotation Matrix 1 * @param R2 Rotation Matrix 2 * @param t Translation * @see Multiple View Geometry in Computer Vision - Result 9.19 p259 */ void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t) { // 【1】對 本質矩陣E 進行奇異值分解 cv::Mat u,w,vt; cv::SVD::compute(E,w,u,vt);// 其中u和v代表二個相互正交矩陣，而w代表一對角矩陣 // 對 t 有歸一化，但是這個地方並沒有決定單目整個SLAM過程的尺度 // 因為CreateInitialMapMonocular函式對3D點深度會縮放，然後反過來對 t 有改變 u.col(2).copyTo(t); t=t/cv::norm(t); // 沿著Z軸旋轉 90度得到的旋轉矩陣（逆時針為正方向） // z 軸還是 原來的z軸 y軸變成原來的 x 軸的負方向 x軸變成原來的y軸 // 所以 旋轉矩陣 為 0 -1 0 // 1 0 0 // 0 0 1 // 沿著Z軸旋轉- 90度 // z 軸還是 原來的z軸 y軸變成原來的 x 軸 x軸變成原來的y軸的負方向 // 所以 旋轉矩陣 為 0 1 0 為上 旋轉矩陣的轉置矩陣 // -1 0 0 // 0 0 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; if(cv::determinant(R1)<0) R1=-R1; R2 = u*W.t()*vt; if(cv::determinant(R2)<0) R2=-R2; } } //namespace ORB_SLAM