ORBSLAM2單目初始化過程

轉自博客：https://blog.csdn.net/zhubaohua_bupt/article/details/78560966

ORB單目模式的初始化過程可以分為以下四個階段：

 

1 通過匹配選取兩個可以作為起始兩幀的初始幀

2 根據匹配計算兩幀之間的位姿

3 三角化測量初始的特征點雲深度，進而獲得點雲地圖。

4 BA優化初始點雲

在初始化后，單目模式和雙目及RGBD模式一樣，都是通過PNP來計算位姿。

 

下面，說一下初始化算法的步驟：

第一階段：選取兩個可以作為起始兩幀的初始幀

這一階段做的工作是，選取是兩個特征點數目大於100的兩個連續幀，並進行匹配，

只有當前后幀匹配點對比較多時（代碼>100），才認為這兩幀可以進行初始化並記錄下來兩幀的匹配關系，

接下來開始嘗試求取兩幀之間的位姿。

否則從頭開始，重新開始嘗試初始化。

第二階段：根據匹配計算兩幀之間的位姿

ORB實現的是自動初始化，也就是，無論場景平面，還是普通場景，都能完成初始化工作。

其做法是同時計算適用於平面場景的單應性矩陣（H）和適用於普通場景的基礎矩陣(F),

方法是：

首先，由抽樣點對，計算出單次抽樣的H和F矩陣（DLT，八點法）；通過若干次RANSAC抽樣，

計算出最優的H和F矩陣；

然后，通過一定機制比較H和F矩陣，選擇最優的矩陣，恢復出來幀間位姿。

並行計算兩個模型：

   在兩個線程上並行計算單映射矩陣模型和基礎矩陣模型 ：

為了使兩個模型的計算流程盡量一樣，用於計算H和F矩陣的迭代循環次數預先設置成一樣（代碼中為200次），

每次用迭代計算的匹配特征點對也是一樣的。基礎矩陣是8個特征點對，單映矩陣是其中的4個特征點對。

 

從若干次RANSAC中，計算出最優的H和F矩陣：

每次迭代中，給每個模型M（H表示單映射，F表示基本矩陣）計算一個分值SM：

 

其中

 

 

TM是無效數據的排除閾值，

它的依據是卡方測試的95%（TH=5.99, TF=3.84，假設在測量誤差上有1個像素的標准偏差）。

 TM等於TH，這樣，對於同一個重投影誤差d，兩個模型求取的分值相同。

模型M越准，所有匹配點對的重投影誤差越小，SM的值就越大。

因此SM的值反映了一個這樣的問題，當其越大時，模型M越准確。

 

我們從單映射矩陣和基本矩陣的計算中選擇分值最高的，分別記作為SH和SF，

但如果兩個模型分值都不高（意味着沒有足夠的局內點），就重新選擇第二幀，重新匹配並嘗試初始化。

模型選擇(用基礎矩陣還是用單應性矩陣？)

       如果場景是平面，近平面或存在低視差的情況，則可以通過單映矩陣來求解。

同樣地，我們也可以找到一個基礎矩陣，但問題是基礎矩陣不能夠很好的約束該問題，而且從基礎矩陣中計算得到的運動結果是錯誤的。在這種情況下，我們應該選擇單映矩陣才能保證地圖初始的正確性，或者如果檢測到低視差的情況則不進行初始化工作。

       另一方面，對於非平面場景且有足夠的視差的情況則可以通過基礎矩陣來計算，

而在這種情況下單映矩陣只有基於平面點或者低視差的匹配點才能找到。因此，

在這種情況下我們應該選擇基礎矩陣。我們利用如下強大的啟發式進行計算：

 

如果RH>0.45 ,這表示二維平面和低視差的情況，我們將選擇計算單應矩陣。其他的情況，我們選擇基礎矩陣。

 

從模型恢復位姿

   一旦選擇好模型，我們就可以獲得相應的運動狀態。如果選擇單映矩陣，我們按照Faugeras等人發表的論文中提到的方法，提取8種運動假設，該方法提出用cheriality測試來選擇有效解。

    然而，如果在低視差的情況下，這些測試就會失效，因為雲點很容易在相機的前面或后面移動，會導致選解錯誤。我們提出的方法是直接按這8種解將二維點三角化，然后檢查是否有一種解可以使得所有的雲點都位於兩個相機的前面，且重投影誤差較小。如果沒有一個最優的解，我們就不執行初始化，否則重新選擇第二幀，重新匹配並嘗試初始化。

這種方法使初始化程序在低視差和兩個交叉的視圖情況下更具魯棒性，這也是我們整個算法體現魯棒性的關鍵所在。

 

在基本矩陣的情況下，我們使用標定矩陣K用下式將其轉換為本證矩陣：

 

然后奇異值分解方法計算4個運動解，然后就像上文中敘述的一樣，我們將四個解用於三角化特征點，以選擇正解。

第三階段：三角化測量初始的特征點雲深度，進而獲得點雲地圖。

 

      一旦恢復出兩幀之間位姿（單目無尺度），就可以求解匹配點對的深度（無尺度），求解時可以以任意一幀為基坐標系，由於這里是要以做SLAM，所以以第一幀為世界坐標系。代碼中采用多視圖幾何書中11.2線性三角形法中的齊次方法（DLT）進行三角測量恢復匹配點對應的三維信息。

      基本原理如下，已知一組匹配點分別在兩幅圖像中的二維坐標，利用叉乘為0可以代表共線的幾何屬性和幀間位姿，構建一個AX=0的方程，其中X是大小4x1向量，是該匹配點對所對應的三維點的齊次表示。然后通過SVD分解來求方程AX=0，對此方程SVD分解本身具有最小二乘效果，可以解決在解方程時，由位姿噪聲帶來的問題。其基本思想和http://blog.csdn.net/zhubaohua_bupt/article/details/74926111一致。

 

第四階段:  對以上恢復的點雲和位姿做 Bundleadjustment

最后執行一個全局BA，以優化初始重構得到的點雲地圖。

以上內容來自於ORBSLAM論文和源碼。