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

---

ORB-SLAM2目錄：

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

---

回顧:
  好久沒更新啦,耽擱了這麼久,實在是最近事情有點多,一直抽不出時間來寫,趁着空閒之際,趕緊更新一波.上一節我們主要講解了關於ORB特徵點的原理以及源碼中的實現,想必讀完上節,大家應該對什麼是ORB,怎麼提取Oriented FAST關鍵點,怎麼計算despritor以及如何進行四叉樹存儲,篩選高質量特徵點,保證特徵點提取的均勻性.
  接下來,有了圖片的特徵點信息之後,我們將正式進入Track();函數進行前端追蹤,估計相機的POSE,創建關鍵幀等等.本文將講解orb-slam2中的單目初始化,由於單目初始化設計內容較多,故將分爲幾次敘述.同時爲了控制每章的規模,不至於文章太長導致讀者們失去了閱讀的興趣.

---

理論環節

本質矩陣E/基礎矩陣F

(1)對極約束

  如上圖所示,空間中一點 $P$ 投影在$camera1$的成像平面$I_1$上的 $p_1$ ,同時投影在$camera2$的成像平面$I_2$上的 $p_2$ .其中, $l_1$ 和 $l_2$ 稱爲極線,$O_1$-$O_2$ 連成的線稱爲基線,由$O_1$-$O_2$-$P$組成的面稱爲極平面.由於投影在平面 $I_1$ 上的點 $p_1$ 擁有無窮多個可能的 $P$ ,且位於$O_1$-$P$ 的射線上.所以 $p_1$經過旋轉變換投影到平面 $I_2$ 上的點 $p_2$ 可能位於極線 $l_2$ 上的任意一點,這就是所謂的對極約束,即由一個點投影后約束到一條線之上,幸好由於前面正確的特徵點匹配,我們已經知道了$p_2$點的確確位置,所以反過來通過三角測量以及最小化重投影誤差來求解 $P$ 點的位置以及兩個 $camera$之間的$POSE$.

   注意:前提必須是正確的特徵點匹配,錯誤的特徵點匹配將導致錯得離譜,到這裏,讀者們應該知道前端的特徵點匹配對於系統的可靠性是有多麼的重要了吧.

  接下來讓我們來看看對極約束的數學表達,假設 $x_1$,$x_2$ 是兩個像素點的歸一化平面座標,從$camera1$到$camera2$的旋轉變換矩陣爲: $T=[R|t]$,所以我們可以得到$x_2=Rx_1+t$將上式同時左乘t^,可得:

   補充: $p^T_2F_{21}p_1=0$ ,可以寫成 $p^T_2l_2=0$ ,表示 $p_2$ 在直線 $l_2$上,表示 $F_{21}$ 將 $p_1$ 投影到幀2圖像中的直線 $l_2$ 上,這樣子講是不是可以更加準確的理解對極約束的物理意義呢?事實上,上面計算出來的基礎矩陣 F 或者本質矩陣 E 都是表示了從幀1到幀2的變化,這在實際代碼編程中將具有重要的意義.

(2)基礎矩陣的計算

   參考"視覺SLAM十四講"中的說明,根據定義矩陣$F$是一個3×3的矩陣,內有9個未知數.那麼,是不是任意一個3×3的矩陣都可以被當成基礎矩陣呢?從F的構造方式上看,有以下值得注意的地方;

• 本質矩陣是由對極約束定義的。由於對極約束是等式爲零的約束,所以對 E 乘以任意非零常數後,對極約束依然滿足.我們把這件事情稱爲 E 在不同尺度下是等價的.
• 本質矩陣 E 的奇異值必定是$[σ, σ, 0]^T$的形式,這稱爲基礎矩陣的內在性質.
• 另一方面,由於平移和旋轉各有三個自由度,故$F$共有六個自由度.但由於尺度等價性,故$F$實際上有五個自由度.
     5個自由度矩陣的求解理論上最少可用五點對,但是實際操作中仍然採用八點法來進行求解,其本質上沒有太大區別.假設一點對分別爲:$p_2=(u_2,v_2,1)$ 和 $p_1=(u_1,v_1,1)$,根據對極約束可得.
  $\begin{bmatrix} u_2 &v_2 &1 \end{bmatrix} \begin{bmatrix} f_1 &f_2 & f_3 \\ f_4 &f_5 &f_6 \\ f_7 &f_8 & f_9 \\ \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}\tag{1}=0$

化簡可得:
$u_1u_2f_1+u_1v_2f_2+u_1f_3+v_1u_2f_4+v_1v_2f_5+v_1f_6+u_2f_7+v_2f_8+f_9=0$

將八點對聯立起來構建成矩陣如下:
$\begin{bmatrix} u_1u_2 &u_1v_2 &u_1 &v_1u_2 &v_1v_2 &v_1 &u_2 &v_2 &1 \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \end{bmatrix} \begin{bmatrix} f_1 \\ f_2 \\f_3 \\f_4 \\f_5 \\f_6 \\f_7 \\f_8 \\f_9 \end{bmatrix}\tag{2} =0$

  由於匹配點之間本身存在誤差,所以式(2)本身不會絕對成立.因此實際求解過程中採用最小特徵值對應的特徵向量來近似替代解.至今,可能很多同學就懵了,這是什麼跟什麼呀…
其實,根據特徵值與特徵向量的定義可知:
$A\xi=\lambda\xi\tag{3}$
當特徵值$\lambda$取0時,等式(3)右邊等於0,左邊的$\xi$不就是矩陣$A\xi=0$的解麼,沒錯,就是我們所要求解的F矩陣.

單應矩陣

(1)概念

   講完了對極約束,我們應該來講講單應矩陣了,單應矩陣也是非常重要的,畢竟對極約束並不能夠解決所有的問題,當相機只有旋轉沒有平移時,可以使用單應矩陣估計相機運動.因爲此時平移爲0,計算出來的E或者F也爲0,進而R=0,得到了錯誤解,而使用單應矩陣依然能夠正確計算.單應矩陣主要用於當相機只有純旋轉運動,或者地圖點在同一平面上時,或許我們可以形象地稱之爲"平面約束".

  本文講的單應矩陣將和"視覺SLAM十四講"中的有略微的區別,主要是用於後面介紹orb-slam2源碼時能夠完全匹配起來,不過並沒有本質上的區別.

  已知一個平面方程 $Ax+By+Cz-d=0$ ,其中該平面的法向量 $n^T=(A,B,C)^T$ ,由於點 $P$ 位於該平面上,故有:$n^TP-d=0$稍加整理,得
$\frac {n^TP} {d}=1$
故
$\begin{aligned} p_2&=K(RP+t)\\ &=K(RP+t\cdot\frac{n^TP}{d})\\ &=K(R+\frac{tn^T}{d})P\\ &=K(R+\frac{tn^T}{d})K^{-1}p_1\\ &=H_{21}p_1 \end{aligned}$
其中,單應矩陣 (注意單應矩陣的下標,表示從$camera1$ 到 $camera2$的單應矩陣變換):
$H=K(R+\frac{tn^T}{d})K^{-1}$

(2)單應矩陣的計算

  單應矩陣採用DLT線性化求解,由於單位矩陣的自由度爲8,而一對匹配特徵點可以提供兩個約束,所以理論上最少可以用四點對進行求解.但是實際應用上其實用四點對或八點對都沒有太大本質上的區別,已知一點對,$p_2=(u_2,v_2,1)$ 和 $p_1=(u_1,v_1,1)$,由單應矩陣變換關係可以得到 (等價於$p_2=H_{21}P_1$);
$\begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix} \tag{4}= \begin{bmatrix} h_1 &h_2 & h_3 \\ h_4 &h_5 & h_6 \\ h_7 &h_8 & h_9 \\ \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}$進一步計算:
$\begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix} \tag{5}= \begin{bmatrix} h_1u_1+h_2v_1+h_3 \\ h_4u_1+h_5v_1+h_6 \\ h_7u_1+h_8v_1+h_9 \\ \end{bmatrix}$所以:
$\left\{\begin{aligned} u_2&=\frac{h_1u_1+h_2v_1+h_3}{h_7u_1+h_8v_1+h_9}\\ v_2&=\frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}\\ \end{aligned}\right.\tag{6}$
可以得到一點對的兩個約束如下 (寫成如此只要是爲了和源碼中的形式相對齊):
$\left\{\begin{aligned} -h_1u_1-h_2v_1-h_3+h_7u_1u_2+h_8v_1u_2+h_9u_2=0\\ h_4u_1+h_5v_1+h_6-h_7v_2u_1-h_8v_2v_1-h_9v_2=0\\ \end{aligned}\right.\tag{7}$
進而按照以上方式計算出八點對,組成如下矩陣求解:
$\begin{bmatrix} 0 &0 &0 &-u_1 &-v_1 &-1 &u_1v_2 &v_1v_2 &v_2 \\ u_1 &v_1 &1 &0 &0 &0 &-u_1u_2 &-u_2v_1 &-u_2 \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\ \end{bmatrix} \begin{bmatrix} h_1 \\ h_2 \\h_3 \\h_4 \\h_5 \\h_6 \\h_7 \\h_8 \\h_9 \end{bmatrix}\tag{8} =0$
至於求解該方程的方式與上述求解F矩陣類似,此處就不再敘述.

根據單應矩陣H / 基礎矩陣F 求解R,t

由於此篇幅較多,故單獨置一章節講解:一步步帶你看懂orbslam2源碼–單應矩陣/基礎矩陣,求解R,t(四)

RANSAC隨機採樣一致性算法

(1)算法流程如下:

  而在orbslam2中,實際操作則是隨機選擇200組8點對進行求解H矩陣和F矩陣,選擇其中$score$最高的H矩陣/F矩陣.

(2)score計算流程:

  根據orbslam2論文中所述,在上述得到了H矩陣之後,利用H矩陣,將P點分別投影到$camera1$和$camera2$中,分別在兩幀圖像中計算P點的重投影誤差(即兩點之間的距離),且該距離的平方必須小於閾值$T_M$纔有效,並將所有匹配點的有效的重投影誤差累加起來 (當然並不是直接將重投影誤差直接相加,而是進行了標準化,後續源碼實現環節將會進行講解),即得到$score$,具體公式如下:

  其中,$d^2$爲重投影誤差(兩點間距離的平方),$d_{cr}$ 和 $d_{rc}$分別對應將P點分別投影到$camera1$和$camera2$中的重投影誤差,$\rho_M(.)$對應這判斷該點的重投影誤差是否小於閾值,小於則有效,結果累加進$score$中,大於則代表無效,捨棄.
  相應的計算基礎矩陣$F$的$score$,同樣採用雙向投影計算,不同的是:計算$p_2$點到直線$F_{21}*p_1$的距離的平方,如果讀者留意到的話,會發現就是計算投影點到極線的距離.

(3)閾值$T_M$的選擇:

  該閾值的選擇主要依賴於卡方分佈統計量 (即正態分佈的平方的累加,累加個數即爲自由度),顯著性水平爲$5\%$,在單應矩陣H中,自由度爲2,對應的閾值$T_H=5.991$,在單應矩陣F中,自由度爲1,對應的閾值$T_F=3.841$.如下圖所示:

模型選擇策略

  orbslam2中採用並行計算單應矩陣和本質矩陣,在算出兩個矩陣對應的$score$之後,按照如下公式計算$R_H$
$R_H=\frac{S_H}{S_H+S_F}$
如果$R_H>0.4$,則選擇採用單應矩陣H恢復相機位姿,否則使用基礎矩陣F.這也說明了orbslam2作者更加傾向於使用單應矩陣進行初始化,單應矩陣對視差小的容忍度比基礎矩陣更好,即更適合位移較小的情形.

---

源碼分析環節

(1)Frame幀創建處理補充

  在講解單目初始化之前,先補充下上節提取完ORB之後的一些處理,其中包括去畸變以及將特徵點分配到對應網格中,分別對應以下兩個函數.

UndistortKeyPoints();
AssignFeaturesToGrid();

我們先來看看去畸變函數,去畸變後的像素點存放於mvKeysUn向量之中,具體代碼如下.

//將像素座標進行去畸變,並存入mvKeysUn
void Frame::UndistortKeyPoints()
{
    if(mDistCoef.at<float>(0)==0.0)
    {
        mvKeysUn=mvKeys;
        return;
    }
    // Fill matrix with points
    cv::Mat mat(N,2,CV_32F);
    for(int i=0; i<N; i++)
    {
        mat.at<float>(i,0)=mvKeys[i].pt.x;
        mat.at<float>(i,1)=mvKeys[i].pt.y;
    }
    // Undistort points
    mat=mat.reshape(2);
    cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
    mat=mat.reshape(1);

    // Fill undistorted keypoint vector
    mvKeysUn.resize(N);
    for(int i=0; i<N; i++)
    {
        cv::KeyPoint kp = mvKeys[i];
        kp.pt.x=mat.at<float>(i,0);
        kp.pt.y=mat.at<float>(i,1);
        mvKeysUn[i]=kp;
    }
}

  首先將mvKeys向量中存放的特徵點座標存入cv::Mat mat(N,2,CV_32F)矩陣之中,接着調用cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);實現去畸變.讓我們來看看OpenCV庫是怎麼實現的吧.

在此源碼中,大致流程總結如下:

  至於源碼中爲何要進行矩陣通道的改變,即先將矩陣轉化爲雙通道,去畸變完成後又將矩陣轉換爲單通道.其實,此處可以進行此操作也可以不進行次操作,因爲undistortPoints()函數的src如下圖所示,既可以是N×2單通道,也可以是N×1雙通道.去畸變後將其座標,又從mat矩陣存入mvKeysUn向量中.

  矯正完成後的特徵點,通過AssignFeaturesToGrid()函數,將其分配到對應的網格中,大致思路是將特徵點的座標轉換爲網格座標,然後判斷該座標是否在網格座標範圍內,如果是,則存入對應的網格,否則拋棄該野點.實現代碼如下:

//將特徵點分配到對應的網格中
void Frame::AssignFeaturesToGrid()
{
    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
            mGrid[i][j].reserve(nReserve);

    for(int i=0;i<N;i++)
    {
        const cv::KeyPoint &kp = mvKeysUn[i];

        int nGridPosX, nGridPosY;
        if(PosInGrid(kp,nGridPosX,nGridPosY))//判斷是否存在nGridPosX,nGridPosY
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}

  到這裏就正式完成了對一幀照片的處理(提取ORB特徵點並進行相應的存儲),接下來我們來討論下如何進行單目初始化.
  我們先看下單目初始化的總體代碼框架:

  讓我們來看看這個函數裏面是什麼東西,點開一看,發現此處調用了一個重載過的括號運算符.

  最開始創建Tracking線程的時候,mpInitializer指針將會賦值爲NULL,直到當前幀中的特徵點數量>100併成功初始化時,纔會執行下面語句進行賦值.因此,只會初始化一次.

mpInitializer =  new Initializer(mCurrentFrame,1.0,200);

關於單目初始化由於篇幅較長,就留到後文繼續講解了…

---

總結

1. 對極約束的原理,以及F矩陣的求解
2. 單應矩陣的原理,以及H矩陣的求解
3. RANSAC隨機採樣一致性算法,閾值選擇原理
4. score計算方式,模型選擇策略
5. 創建Frame時源碼的部分補充

參考文獻:

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

PS:

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

 
 
上一章節:一步步帶你看懂orbslam2源碼–orb特徵點提取(二)
下一章節:一步步帶你看懂orbslam2源碼–單應矩陣/基礎矩陣,求解R,t(四)