ORB-SLAM2 論文&代碼學習 —— 單目初始化

轉載請註明出處，謝謝
原創做者：Mingrui
原創連接：http://www.javashuo.com/article/p-gmtyqhmy-hz.html

---

本文要點：

• ORB-SLAM2 單目初始化部分 論文內容介紹
• ORB-SLAM2 單目初始化部分 代碼結構介紹

寫在前面

以前的 ORB-SLAM2 系列文章中，咱們已經對 Tracking 線程作了介紹，可是當時咱們跳過了 Tracking 線程中一個很重要的部分 —— 單目初始化。咱們將在本文中，對 ORB-SLAM2 系統的單目初始化部分進行介紹。

依舊祭出該圖，方便查看：

也再次獻上我繪製的程序導圖全圖：ORB-SLAM2 程序導圖

老規矩，仍是分兩部分：以 ORB-SLAM 論文爲參考 和 以 ORB-SLAM2 代碼（程序導圖）爲參考。

以 ORB-SLAM 論文爲參考

對於任何一個單目 SLAM 系統來講，在系統運行之初都要進行初始化，其目的在於，要計算出某兩幀的相對位姿，以此來經過三角化獲得一些初始 MapPoints，從而獲得一個初始的 Map，這樣以後的跟蹤也好優化也好都在這個基礎上進行。在 ORB-SLAM 以前的單目 SLAM 系統的初始化，每每須要依賴真實場景中某樣肯定的物體（eg. MonoSLAM）或者須要人工介入（eg. PTAM），而 ORB-SLAM 的單目初始化是徹底自動的。

對 SLAM 基礎知識有過了解的同窗會知道，恢復兩幀之間的相對運動，有兩種模型：基礎矩陣（Fundamental）（等同於本質矩陣）模型和單應矩陣（Homography）模型。正常狀況下基礎矩陣模型應該能夠應付，但若是特徵點共面（初始化場景中主要是一個平面），或者兩幀之間的相對位姿未純旋轉時，基礎矩陣的自由度會降低，也就是所謂的退化，相似於方程數少於變量數。此時爲了保證運動恢復的精度，就不能再用基礎矩陣模型。由此提出了單應矩陣，其假設特徵點落在同一平面上，從而適用於這種場景下的運動恢復。

ORB-SLAM 在初始化時，它也不知道場景中的特徵點在不在同一平面，因此它選擇兩種模型各自算一遍（開兩個線程同時算），以後計算兩種模型各自進行運動恢復的得分，取得分高的模型，再根據該模型，計算兩幀之間的相對位姿並進行初始化。

單目初始化具體步驟

1. 提取 ORB 特徵點

在當前幀和參考幀中提取 FeaturePoints（只在最優的尺度），同時將當前幀和參考幀的 FeaturePoints 作匹配。若是匹配點不夠多，從新初始化。

2. 同時計算兩種模型

• 單應矩陣 \(H_{cr}\)：DLT 算法 + RANSAC 迭代
• 基礎矩陣 \(F_{cr}\)：8點法 + RANSAC 迭代

同時計算兩種模型各自的得分，計算公式以下（其中 M 能夠表示 H (Homography)，也能夠表示 F (Fundamental)）：

\(S_{M}=\sum_{i}\left(\rho_{M}\left(d_{c r}^{2}\left(\mathbf{x}_{c}^{i}, \mathbf{x}_{r}^{i}, M\right)\right)+\rho_{M}\left(d_{r c}^{2}\left(\mathbf{x}_{c}^{i}, \mathbf{x}_{r}^{i}, M\right)\right)\right)\)

\(\rho_{M}\left(d^{2}\right)=\left\{\begin{array}{ll} {\Gamma-d^{2}} & {\text { if } \quad d^{2}<T_{M}} \\ {0} & {\text { if } \quad d^{2} \geq T_{M}} \end{array}\right.\)

其中 \(d_{c r}^{2}\) \(d_{r c}^{2}\) 是對稱轉換偏差。\(\rho_{M}()\) 的做用是令大的偏差對低的得分，其中 \(T_M\) 閾值是算出來的：假設測量值偏差的標準差爲1像素，經過95%的 \(\chi^{2}\) 檢驗獲得的。對於每種模型，在 RANSAC 迭代過程當中保留得分最高的模型。若是最終沒能求出解（對於 RANSAC，inliers不夠多），則從新初始化。

3. 選擇合適的模型

根據兩種模型各自的得分：

\(R_{H}=\frac{S_{H}}{S_{H}+S_{F}}\)

若是 \(R_H\) > 0.45（代碼中是 0.4），則選用單應矩陣；反之則選基礎矩陣模型。大概就是選得分高的，此處的 0.4 應該是經驗值。

4. 進行運動恢復，求解兩幀相對位姿（\(R, t\)）並經過三角化獲得初始 MapPoints

在求出 \(H_{cr}\) 或 \(F_{cr}\) 後，就要根據該矩陣求出相對位姿（\(R, t\)）。但這個過程的求出的解不是惟一的。ORB-SLAM 採起的篩選辦法是：對這些解所有進行三角化恢復 MapPoints，哪一個解恢復出來的 MapPoints 大部分都在相機前方且重投影偏差小，就選哪一個解。若是不能明確選出一個最合適的，則從新初始化。

5. BA 優化

最後，進行一次全局 BA，優化如下，獲得最終的初始化結果。

從以上步驟能夠看出，ORB-SLAM 在單目初始化花了不少心思。有一個很明顯的特色：只要出現一點不妥，ORB-SLAM 就會選擇從新初始化。論文中說，這種高標準嚴要求的初始化準則，是 ORB-SLAM 系統魯棒性很是好的重要緣由之一。由於若是初始化就不合適或出錯，後面的跟蹤只會一錯再錯，錯上加錯。

以 ORB-SLAM2 代碼（程序導圖爲參考）

在上圖中 MonocularInitialization() 就是初始化的程序，咱們能夠看到它在 Tracking 線程中的位置。

上圖是 MonocularInitialization() 部分的程序框圖，其大致和論文中介紹的步驟是徹底一致的，這樣圖已經很清晰了，這裏就很少描述了。

若是嫌這張圖不夠清晰的話，能夠點擊 ORB-SLAM2 程序導圖連接（文首）查看清晰全圖

PS： 從中上圖咱們能夠看到，在恢復出兩幀之間的相對運動後，程序中還要使用 Tracking::CreateInitialMapMonocular() 來創建初始化的地圖。這部分在論文裏幾乎沒有筆墨提到，但在程序裏須要很大篇幅來實現。這個細節就反映了我在本系列博文開篇就提到過的讀通 ORB-SLAM2 代碼的困難之處。以我如今小菜雞的水平，我根本想象不出 ORB-SLAM2 這樣複雜而環環相扣的工程是怎麼寫出來的（流淚）。

ORB-SLAM2 系列博文

ORB-SLAM2 初體驗 —— 配置安裝

ORB-SLAM2 論文&代碼學習 —— 概覽

ORB-SLAM2 論文&代碼學習 —— Tracking 線程

ORB-SLAM2 論文&代碼學習 —— 單目初始化