對于雙目、RGB-D相機，可獲得深度，因此不存在尺度問題，因此Sim3中的尺度s=1。
（1）通過詞袋加速算法實現當前幀、閉環幀的特征點的匹配，建立閉環幀的路標點和當前幀的特征點間的聯系。
（2）使用RANSAC法，隨機采取3對點（根據特征點的索引，獲得當前幀中的路標點（局部建圖時獲得）及對應的閉環幀中的路標點（步驟（1）中獲得）），計算兩幀間的Sim3變換。一共迭代5次，如果有一次，獲得的內點數大于20，就認為成功。
（3）根據Sim3變換，將閉環幀的路標點投影至當前幀中進行匹配。隨后優化Sim3矩陣。
（4）將閉環幀及其共視幀的路標點投影至當前幀中，如果匹配上的路標點大于40，就認為該閉環幀可靠。
（5）隨后就可以進行閉環校正，根據當前幀和閉環幀的Sim3矩陣，去校正當前幀及其共視幀的位姿、路標點坐標。更新共視關系，更新本質圖。

設置RANSAC采樣參數

主要參數包括迭代次數、迭代成功時，內點的數目、K次迭代可以成功的概率。

void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations)
{mRansacProb = probability;              // 0.99mRansacMinInliers = minInliers;         // 20mRansacMaxIts = maxIterations;          // 最大迭代次數 300// 匹配點的數目N = mvpMapPoints1.size(); // number of correspondences// 內點標記向量mvbInliersi.resize(N);// Adjust Parameters according to number of correspondencesfloat epsilon = (float)mRansacMinInliers/N;// Set RANSAC iterations according to probability, epsilon, and max iterations // 計算迭代次數的理論值，也就是經過這么多次采樣，其中至少有一次采樣中,三對點都是內點// epsilon 表示了在這 N 對匹配點中,我隨便抽取一對點是內點的概率; // 為了計算Sim3,我們需要從這N對匹配點中取三對點;那么如果我有放回的從這些點中抽取三對點,取這三對點均為內點的概率是 p0=epsilon^3// 相應地,如果取三對點中至少存在一對匹配點是外點, 概率為p1=1-p0// 當我們進行K次采樣的時候,其中每一次采樣中三對點中都存在至少一對外點的概率就是p2=p1^k// K次采樣中,至少有一次采樣中三對點都是內點的概率是p=1-p2// 候根據 p2=p1^K 我們就可以導出 K 的公式：K=\frac{\log p2}{\log p1}=\frac{\log(1-p)}{\log(1-epsilon^3)}// 也就是說，我們進行K次采樣,其中至少有一次采樣中,三對點都是內點; 因此我們就得到了RANSAC迭代次數的理論值int nIterations;if(mRansacMinInliers==N)        nIterations=1; // 這種情況的時候最后計算得到的迭代次數的確就是一次elsenIterations = ceil(log(1-mRansacProb)/log(1-pow(epsilon,3)));   // 外層的max保證RANSAC能夠最少迭代一次;// 內層的min的目的是,如果理論值比給定值要小,那么我們優先選擇使用較少的理論值來節省時間(其實也有極大概率得到能夠達到的最好結果);// 如果理論值比給定值要大,那么我們也還是有限選擇使用較少的給定值來節省時間mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));// 當前正在進行的迭代次數mnIterations = 0;
}

?
?

RANSAC求解Sim3變換

求解成功的標志：內點數大于20。

/*** @brief Ransac求解mvX3Dc1和mvX3Dc2之間Sim3，函數返回mvX3Dc2到mvX3Dc1的Sim3變換* * @param[in] nIterations           設置的最大迭代次數* @param[in] bNoMore               為true表示窮盡迭代還沒有找到好的結果，說明求解失敗* @param[in] vbInliers             標記是否是內點* @param[in] nInliers              內點數目* @return cv::Mat                  計算得到的Sim3矩陣*/
cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)

開始循環

循環條件：當前迭代次數小于5，總的迭代次數小于300。當前迭代次數指的是對于當前幀嘗試求解Sim3的次數，如果大于5了還不行，就取消?？偟牡螖抵傅氖?#xff1a;在回環檢測的時候，會計算所有的候選閉環幀和當前幀的Sim3，在這個過程中的總的迭代次數。
（1）隨機取3組點，并將索引在列表中刪除

        // Get min set of points// Step 2.1 隨機取三組點，取完后從候選索引中刪掉for(short i = 0; i < 3; ++i){// DBoW3中的隨機數生成函數int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);int idx = vAvailableIndices[randi];// P3Dc1i和P3Dc2i中點的排列順序：// 相機坐標// x1 x2 x3 ...// y1 y2 y3 ...// z1 z2 z3 ...mvX3Dc1[idx].copyTo(P3Dc1i.col(i));mvX3Dc2[idx].copyTo(P3Dc2i.col(i));// 從"可用索引列表"中刪除這個點的索引 vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}

（3）根據匹配的3對路標點，計算Sim3變換

ComputeSim3(P3Dc1i,P3Dc2i);

（4）根據Sim3變換檢測內點數量

CheckInliers();

（5）更新最優的Sim3參數系數，包括R，t，s，匹配上的內點數等。如果內點數大于20，則計算成功，直接返回Sim3矩陣。

        // Step 2.4 記錄并更新最多的內點數目及對應的參數if(mnInliersi>=mnBestInliers){mvbBestInliers = mvbInliersi;mnBestInliers = mnInliersi;mBestT12 = mT12i.clone();mBestRotation = mR12i.clone();mBestTranslation = mt12i.clone();mBestScale = ms12i;if(mnInliersi>mRansacMinInliers) // 只要計算得到一次合格的Sim變換，就直接返回{// 返回值,告知得到的內點數目nInliers = mnInliersi;for(int i=0; i<N; i++)if(mvbInliersi[i])// 標記為內點vbInliers[mvnIndices1[i]] = true;return mBestT12;} // 如果當前次迭代已經合格了,直接返回} // 更新最多的內點數目

上述過程重復5次還不行，就直接返回FALSE

    // Step 3 如果已經達到了最大迭代次數了還沒得到滿足條件的Sim3，說明失敗了，放棄，返回if(mnIterations>=mRansacMaxIts)bNoMore=true;

?
?

計算三個點的質心以及去質心坐標

/*** @brief 給出三個點,計算它們的質心以及去質心之后的坐標* * @param[in] P     輸入的3D點* @param[in] Pr    去質心后的點* @param[in] C     質心*/
void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C)
{// 矩陣P每一行求和，結果存在C。這兩句也可以使用CV_REDUCE_AVG選項來實現cv::reduce(P,C,1,CV_REDUCE_SUM);C = C/P.cols;// 求平均for(int i=0; i<P.cols; i++){Pr.col(i)=P.col(i)-C;//減去質心}
}

?
?

根據3對匹配的點計算Sim3

需要3對相機1、相機2中的匹配路標點（相機坐標系中）
旋轉矩陣------尺度------平移矩陣-------構建Sim3矩陣

計算三個點的質心

    cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)cv::Mat O1(3,1,Pr1.type()); // Centroid of P1cv::Mat O2(3,1,Pr2.type()); // Centroid of P2ComputeCentroid(P1,Pr1,O1);ComputeCentroid(P2,Pr2,O2);

計算M、N矩陣

    // Step 2: 計算論文中三維點數目n>3的 M 矩陣。這里只使用了3個點// Pr2 對應論文中 r_l,i'，Pr1 對應論文中 r_r,i',計算的是P2到P1的Sim3，論文中是left 到 right的Sim3cv::Mat M = Pr2*Pr1.t();// Step 3: 計算論文中的 N 矩陣double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;cv::Mat N(4,4,P1.type());N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);   // Sxx+Syy+SzzN12 = M.at<float>(1,2)-M.at<float>(2,1);                    // Syz-SzyN13 = M.at<float>(2,0)-M.at<float>(0,2);                    // Szx-SxzN14 = M.at<float>(0,1)-M.at<float>(1,0);                    // ...N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);N23 = M.at<float>(0,1)+M.at<float>(1,0);N24 = M.at<float>(2,0)+M.at<float>(0,2);N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);N34 = M.at<float>(1,2)+M.at<float>(2,1);N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,N12, N22, N23, N24,N13, N23, N33, N34,N14, N24, N34, N44);

計算旋轉矩陣

    // Step 4: 特征值分解求最大特征值對應的特征向量，就是我們要求的旋轉四元數cv::Mat eval, evec;  // val vec// 特征值默認是從大到小排列，所以evec[0] 是最大值// 計算對稱矩陣的特征值，特征向量cv::eigen(N,eval,evec); // N 矩陣最大特征值（第一個特征值）對應特征向量就是要求的四元數（q0 q1 q2 q3），其中q0 是實部// 將(q1 q2 q3)放入vec（四元數的虛部）cv::Mat vec(1,3,evec.type());(evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)// Rotation angle. sin is the norm of the imaginary part, cos is the real part// 四元數虛部模長 norm(vec)=sin(theta/2), 四元數實部 evec.at<float>(0,0)=q0=cos(theta/2)// 這一步的ang實際是theta/2，theta 是旋轉向量中旋轉角度// ? 這里也可以用 arccos(q0)=angle/2 得到旋轉角吧double ang=atan2(norm(vec),evec.at<float>(0,0));// vec/norm(vec)歸一化得到歸一化后的旋轉向量,然后乘上角度得到包含了旋轉軸和旋轉角信息的旋轉向量vec// vec：旋轉向量vec = 2*ang*vec/norm(vec); //Angle-axis x. quaternion angle is the halfmR12i.create(3,3,P1.type());// 旋轉向量（軸角）轉換為旋轉矩陣cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis

計算s

雙目、RGB-D的s為1。

    // Step 6: 計算尺度因子 Scaleif(!mbFixScale){// 論文中有2個求尺度方法。一個是p632右中的位置，考慮了尺度的對稱性// 代碼里實際使用的是另一種方法，這個公式對應著論文中p632左中位置的那個// Pr1 對應論文里的r_r,i',P3對應論文里的 r_l,i',(經過坐標系轉換的Pr2), n=3, 剩下的就和論文中都一樣了double nom = Pr1.dot(P3);// 準備計算分母cv::Mat aux_P3(P3.size(),P3.type());aux_P3=P3;// 先得到平方(每個元素都平方)cv::pow(P3,2,aux_P3);double den = 0;// 然后再累加for(int i=0; i<aux_P3.rows; i++){for(int j=0; j<aux_P3.cols; j++){den+=aux_P3.at<float>(i,j);}}ms12i = nom/den;}elsems12i = 1.0f;

計算平移向量

    // Step 7: 計算平移Translationmt12i.create(1,3,P1.type());// 論文中平移公式mt12i = O1 - ms12i*mR12i*O2;

計算1–>2,2–>1的Sim3變換矩陣

    // Step 8: 計算雙向變換矩陣，目的是在后面的檢查的過程中能夠進行雙向的投影操作// Step 8.1 用尺度，旋轉，平移構建變換矩陣 T12mT12i = cv::Mat::eye(4,4,P1.type());cv::Mat sR = ms12i*mR12i;//         |sR t|// mT12i = | 0 1|sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));mt12i.copyTo(mT12i.rowRange(0,3).col(3));// Step 8.2 T21mT21i = cv::Mat::eye(4,4,P1.type());cv::Mat sRinv = (1.0/ms12i)*mR12i.t();sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));cv::Mat tinv = -sRinv*mt12i;tinv.copyTo(mT21i.rowRange(0,3).col(3));

?
?

檢測內點

void Sim3Solver::CheckInliers()

投影路標點

根據Sim3變換，將1中的路標點投影至2中，2中的路標點投影至1中。

    // 用計算的Sim3 對所有的地圖點投影，得到圖像點vector<cv::Mat> vP1im2, vP2im1;Project(mvX3Dc2,vP2im1,mT12i,mK1);// 把2系中的3D經過Sim3變換(mT12i)到1系中計算重投影坐標Project(mvX3Dc1,vP1im2,mT21i,mK2);// 把1系中的3D經過Sim3變換(mT21i)到2系中計算重投影坐標

根據重投影誤差確定內點

    // 對于兩幀的每一個匹配點for(size_t i=0; i<mvP1im1.size(); i++){// 當前關鍵幀中的地圖點直接在當前關鍵幀圖像上的投影坐標mvP1im1，mvP2im2// 對于這對匹配關系,在兩幀上的投影點距離都要進行計算cv::Mat dist1 = mvP1im1[i]-vP2im1[i];cv::Mat dist2 = vP1im2[i]-mvP2im2[i];// 取距離的平方作為誤差const float err1 = dist1.dot(dist1);const float err2 = dist2.dot(dist2);// 根據之前確定的這個最大容許誤差來確定這對匹配點是否是外點if(err1<mvnMaxError1[i] && err2<mvnMaxError2[i]){mvbInliersi[i]=true;mnInliersi++;}elsemvbInliersi[i]=false;}// 遍歷其中的每一對匹配點

總結

以上是生活随笔為你收集整理的ORB_SLAM2中的Sim3变换的全部內容，希望文章能夠幫你解決所遇到的問題。

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