一步步帶你看懂orbslam2源碼--單目初始化(五)

ORB-SLAM2目錄：

一步步帶你看懂orbslam2源碼–總體框架(一)
一步步帶你看懂orbslam2源碼–orb特徵點提取(二)
一步步帶你看懂orbslam2源碼–單目初始化(三)
一步步帶你看懂orbslam2源碼–單應矩陣/基礎矩陣,求解R,t(四)
一步步帶你看懂orbslam2源碼–單目初始化(五)

回顧:
前面我們已經講解了orbslam2中的理論環節,包括了RANSAC隨機採樣一致性算法,閾值的選擇緣由,對極約束的原理,單應矩陣和基礎矩陣的計算,如何從單應矩陣或基礎矩陣中分解出R,t,單目初始化單應矩陣或基礎矩陣選擇策略等.本章節我們將主要進行單目初始化的理論環節,話不多說,接下來就直接進入正題.

理論環節

先貼上Track()函數的整體框架代碼,首先第一次執行時,mState將會默認等於NO_IMAGES_YET,也就是說系統將會進行一次初始化,然後初始化成功之後的每次運行,系統將不會再次進入if(mState==NOT_INITIALIZED)處的內容 (除非跟蹤丟失,重定位?還沒看完,暫時不清楚) ,即每次將會進入else{…}部分進行前端跟蹤.

由於本系列教程,我們是針對單目SLAM進行講解的,所以接下來我們將進入MonocularInitialization();進行單目初始化.

同樣的,我們先來看來看一下該函數的主要框架.首先,系統將會選擇一張圖像特徵點數量大於100的圖像作爲參考幀,並將該幀中的特徵點作爲預匹配特徵點存入mvbPrevMatched向量中.什麼意思呢?什麼叫做預匹配點呢?其實由於單目初始化時,系統假設在兩幀之間的位移很少(即對於參考幀中的特徵點對應幀2中的匹配點,該匹配點與幀1特徵點位置很接近),所以在尋找幀2匹配點時,將以幀1中特徵點的位置的周邊一小塊區域作爲搜索對象.不知道同學們聽懂了沒有…emm…假設大家都已經聽懂了…下面給出總體框架的流程圖

也就是說,系統將會選擇當前幀作爲參考幀(如果該幀特徵點數量>100),然後與下幀進行匹配,如果匹配失敗,則將更新參考幀,並再與下幀進行匹配,以此類推,直至匹配成功.
接下來我們將講解如下函數:

// Find correspondences
ORBmatcher matcher(0.9,true);
//輸入參數:mInitialFrame,mCurrentFrame這是待匹配的兩幀圖片
//mvbPrevMatched爲預匹配點座標,函數運算後將會更新爲真正的匹配點座標
//mvIniMatches爲mInitialFrame幀中的特徵點在mCurrentFrame幀中的匹配點的索引
//100爲搜索的區域邊長,源碼中是正方形的邊長
//return 成功匹配的數量
int nmatches=matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

讓我們來看看SearchForInitialization()函數的總體流程圖吧

該函數篇幅相對較長,但是基本上都不會怎麼難,這裏只是給出了整體的操作流程,相信讀者們可以自行閱讀懂該函數.好了,當匹配成功之後,系統將會執行以下程序計算相機的POSE,並且創建初始地圖.

        //匹配成功,計算POSE
        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)

        if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
        {
            //剔除三角測量失敗的匹配點
            for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
            {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }

            // Set Frame Poses
            //初始幀pose設置爲單位矩陣
            //更新當前幀pose
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);

            CreateInitialMapMonocular();
        }

以上程序的難點主要在於以下函數

//mvIniMatches:幀1特徵點在幀2中的匹配
//Rcw,tcw:待求相機位姿
//mvIniP3D:成功匹配點的空間3D座標
//vbTriangulated:成功三角測量--ture,三角測量失敗--false
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)

同樣地,我們來看下這個函數的流程圖.

如下爲創建組隨機8點對的程序,如果大家沒忘記的話,這不就是RANSAC隨機採樣一致性算法中的選取若干個最小子集?以下程序實現隨機選取的思路大致如下:首先利用隨機種子選擇vAvailableIndices.size()範圍內的一個索引值(vAvailableIndices.size()這玩意不就是成功匹配點的數量?),然後利用vAvailableIndices[randi] = vAvailableIndices.back()和 vAvailableIndices.pop_back();分別實現:將vAvailableIndices向量中的最後一位放置於已被抽選的索引值處;將vAvailableIndices向量中的最後一位刪除.這樣子就實現了vAvailableIndices向量一直保存爲未抽取到的索引值,且向量大小逐漸減小.

    // Generate sets of 8 points for each RANSAC iteration
    //創建具有 mMaxIterations 個八點對向量的向量
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    DUtils::Random::SeedRandOnce(0);

    //隨機選取 mMaxIterations 組,每組包含8個點,存入 mvSets 中
    for(int it=0; it<mMaxIterations; it++)
    {
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            int idx = vAvailableIndices[randi];

            mvSets[it][j] = idx;

            //刪除已被選的索引
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

選取好mMaxIterations組8點對之後,將創建兩個臨時線程,並行計算單應矩陣和基礎矩陣,其中vbMatchesInliersH,SH,H爲傳入參數引用,目的是爲了更新這兩個值.

    // Launch threads to compute in parallel a fundamental matrix and a homography
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF;
    cv::Mat H, F;

    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

這裏就稍微補充點C++知識吧,這是創建線程並啓動線程的方式:

std::thread m_thrProcess(&類名::函數名, &類對象, 函數參數);

至於爲什麼函數參數前面要加上ref()呢?std::ref只是嘗試模擬引用傳遞，並不能真正變成引用，在非模板情況下，std::ref根本沒法實現引用傳遞，只有模板自動推導類型時，ref能用包裝類型reference_wrapper來代替原本會被識別的值類型，而reference_wrapper能隱式轉換爲被引用的值的引用類型。比如thread的方法傳遞引用的時候，必須外層用ref來進行引用傳遞，否則就是淺拷貝。而在下方添加threadH.join()時,所在線程將會被阻塞,直至線程完成,也就是說只有計算完H和F矩陣,程序纔會運行後面的內容.

言歸正傳,接下來讓我們來看看FindHomography這個函數吧,至於FindFundamental這個函數,在這裏筆者就不講了,非常類似,在有了前面所講的理論基礎之後,相信同學們應該可以沒什麼障礙的讀懂.

進入FindHomography()函數,我們可以發現其中先將特徵點進行歸一化,再利用歸一化後的座標進行計算單應矩陣,而後再換算成原座標的單應矩陣,至於爲什麼使用歸一化的座標呢?其實筆者也不是特別理解,希望路過的讀者們誰知道的麻煩傳授下筆者,感激不盡~~
歸一化公式如下所示:
$u^{'}=\frac{u-u_{mean}}{\sum_{i=1}^N \left|u-u_{mean}\right|/N}\tag{1}$
$v^{'}=\frac{v-v_{mean}}{\sum_{i=1}^N \left|v-v_{mean}\right|/N}\tag{2}$
經過公式(1)和公式(2)的換算之後,就可以將原特徵點的座標歸一化爲均值爲0,一階絕對矩爲1.令
$sX=\sum_{i=1}^N \left|u-u_{mean}\right|/N$
$sY=\sum_{i=1}^N \left|v-v_{mean}\right|/N$
可以得到原座標和歸一化座標之間的歸一化矩陣表示(歸一化座標=T*原座標):
$\begin{bmatrix} u^{'} \\ v^{'} \\ 1 \end{bmatrix} \tag{3}= \begin{bmatrix} sX &0 & -u_{mean}\cdot sX \\ 0 &sY & -v_{mean}\cdot sY \\ 0 &0 & 1 \\ \end{bmatrix} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} (u-u_{mean})\cdot sX \\ (v-v_{mean})\cdot sY \\ 1 \end{bmatrix}$
由先前講解的單應矩陣的計算可知:
$p_2^{'}=H_{21}^{'}p_1^{'}\tag{4}$
式(4)中使用的是皆爲歸一化的特徵點座標,求解出來的自然也是歸一化後的單應矩陣,因此原座標的單應矩陣計算公式如下:
$T_2p_2=H_{21}^{'}T_1p_1$
其中 $T_1$ 和 $T_2$ 分別爲幀1和幀2中對應的歸一化矩陣.因此可得:
$p_2=T_2^{-1}H_{21}^{'}T_1p_1$
故原座標的單應矩陣爲:
$H_{21}=T_2^{-1}H_{21}^{'}T_1\tag{5}$

以下爲核心代碼,實現思路是使用mMaxIterations組隨機八點對計算H矩陣,並調用 currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);函數計算該矩陣的分數,最後存儲最高分數的單應矩陣作爲最終結果.

    // Perform all RANSAC iterations and save the solution with highest score
    //從若干組解中尋找最優H
    for(int it=0; it<mMaxIterations; it++)
    {
        // Select a minimum set
        //將8個點中 幀1索引 和 幀2索引 分別存入 vPn1i 和 vPn2i
        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];
        }

        //通過標準化逆矩陣和標準化點對的單應變換計算原點對的單應變換矩陣
        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
        H21i = T2inv*Hn*T1;//單應矩陣
        H12i = H21i.inv();

        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);

        if(currentScore>score)
        {
            H21 = H21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }

其中ComputeH21()函數如下,如果大家還麼忘記的話,應該知道最後算出來的解就是最小特徵值對應的特徵向量吧…emm…本來不想貼上這段代碼的,沒什麼東西,不過主要是爲了提醒大家這個點…

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{
    const int N = vP1.size();

    cv::Mat A(2*N,9,CV_32F);

    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;

        //爲啥v2  取了相反數(雖然好像無所謂)
        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=U*W*Vt
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

    return vt.row(8).reshape(0, 3);
}

接下來我們來看看currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);函數的細節吧,orbslam2中採用了雙向重投影誤差,計算公式如下:
$score=\sum_{i=1}^N(2\cdot th-\frac{\begin{Vmatrix} p_2^{1} -p_1^{1} \end{Vmatrix}^2+\begin{Vmatrix} p_2^2 -p_1^2 \end{Vmatrix}^2}{\sigma^2})$
$if \frac{\begin{Vmatrix} p_2-p_1 \end{Vmatrix}^2}{\sigma^2}≤th\\ then,該點爲inlines,否則爲outliners且不算入上述score計算$
其中, $p_2^1$ 表示幀2中的匹配點經過H矩陣投影在幀1中的點, $p_1^1$ 表示幀1中的原特徵點,以此類推.實現代碼如下:

/** 利用H矩陣計算reprojection error 
    score=the sum of all matches(2*th-(||p2-p1||^2+ ||p1-p2||^2)/sigma^2)
    if ||p2-p1||^2/sigma^2 or ||p1-p2||^2/sigma^2 > th
    then set the point outliners
    return score
**/
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
{   
    const int N = mvMatches12.size();

    const float h11 = H21.at<float>(0,0);
    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);

    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);

    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;

        // Reprojection error in first image
        // x2in1 = H12*x2

        const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
        const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
        const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;

        const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);//2範數的平方

        const float chiSquare1 = squareDist1*invSigmaSquare;

        if(chiSquare1>th)
            bIn = false;
        else
            score += th - chiSquare1;

        // Reprojection error in second image
        // x1in2 = H21*x1

        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;
}

至於基礎矩陣F的求解過程與單應矩陣求解類似,主要說說兩者之間的不同.

單應矩陣由於投影過去是一個點,所以雙向重投影誤差算的是兩點之間的距離,而基礎矩陣一個點投影過去得到是一條極線,因此雙向重投影誤差算的是點到線之間的距離.
單應矩陣的th是5.991,而基礎矩陣的th是3.841,但是最終在計算分數時都是統一使用5.991,這當然是爲了讓結果有可比性.
由於基礎矩陣具有特徵值爲: $[σ, σ, 0]^T$ 的內在性質,所以計算出來的F矩陣會進行奇異值分解,並且調整特徵值的大小,源碼中是直接讓最後一位奇異值爲0,雖然也可以這樣 $[\frac{σ_1+σ_2}{2}, \frac{σ_1+σ_2}{2}, 0]^T$
基礎矩陣的分數計算公式:
$score=\sum_{i=1}^N(2\cdot th-\frac{\begin{Vmatrix} \frac{a_2 u_2+b_2v_2+c_2}{\sqrt{a_2^2+b_2^2}} \end{Vmatrix}^2+\begin{Vmatrix} \frac{a_1 u_1+b_1v_1+c_1}{\sqrt{a_1^2+b_1^2}} \end{Vmatrix}^2}{\sigma^2})$

其中 $\begin{Vmatrix} \frac{a_1 u_1+b_1v_1+c_1}{\sqrt{a_1^2+b_1^2}} \end{Vmatrix}^2$ 不正是點到線距離的計算公式麼?第四點的分數部分計算源碼如下:

        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 second image
        // l2=F21x1=(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;

        const float num2 = a2*u2+b2*v2+c2;

        const float squareDist1 = num2*num2/(a2*a2+b2*b2);//p2點到直線F21*p1的距離的平方

至此,我們已經講解完單目初始化的絕大部分內容,除了初始地圖構建,關鍵幀插入,該部分內容留到下章繼續講解~~

總結

該章節主要是對源碼進行解說,其中包括:F矩陣的求解,H矩陣的求解
RANSAC隨機採樣一致性算法源碼實現
score計算方式源碼實現

參考文獻:

ORB-SLAM a Versatile and Accurate Monocular SLAM System
高翔–視覺SLAM十四講
吳博-ORB-SLAM代碼詳細解讀
其他博主的文章:https://blog.csdn.net/hzwwpgmwy/article/details/83578694

PS:

如果您覺得我的博客對您有所幫助，歡迎關注我的博客。此外，歡迎轉載我的文章，但請註明出處鏈接。
本博客僅代表個人觀點,不一定完全正確,如有出錯之處,也歡迎批評指正.

上一章節:一步步帶你看懂orbslam2源碼–單應矩陣/基礎矩陣,求解R,t(四)
下一章節:一步步帶你看懂orbslam2源碼–地圖初始化\關鍵幀插入(六)–待更新…

一步步帶你看懂orbslam2源碼--單目初始化(五)

理論環節

總結

參考文獻:

PS:

leetCode---尋找兩個有序數組的中位數

SLAM中CMakeLists常見庫的相關配置

如何實現VMware虛擬機上的ubuntu與windows主機共享文件夾

leetCode--最長迴文子串

控制科學與工程：隨手筆記（5）--Eigen、vscode配置

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結