ORB-SLAM: A Versatile and Accurate Monocular SLAM System 筆記（二）
4. 自動地圖初始化
地圖初始化的目標是兩個幀之間相對位姿來三角化一系列的點雲（riangulate an initial set of map points），這個操作是獨立與場景且不需要人為的幹預的。文章提出平行的計算兩個幾何模型：對平面場景的 homography 和對非平面的 fundamental matrix 。然後根據實際情況選擇一個模型做初始化，算法的步驟如下：
Find initial correspondences: 從當前幀 \(F_c\) 中提取 ORB 特征並且在相鄰幀 \(F_r\) 尋找匹配 \(\bf x_c \leftrightarrow \bf x_r\)
，如果沒有找到足夠的匹配，就重置參考幀。Parallel computation of the two models: 同時計算 homography \(\bf H_{cr}\) 和 fundamental matrix \(\bf F_{cr}\) ：
\[ {\bf x}_c = {\bf H}_{cr} \, {\bf x}_r, \quad {\bf x}_c^T \, {\bf F}_{cr} \, {\bf x}_r = 0 \]
計算模型使用的特征點和叠代數是提前固定的，每次叠代都給模型打分：
\[ \begin{eqnarray} S_M &=& \sum _i \big (\rho _M \big (d_{cr}^2({\bf x}_c^i,{\bf x}_r^i,M)\big) \nonumber +\; \rho _M (d_{rc}^2\big ({\bf x}_c^i,{\bf x}_r^i,M)\big) \big) \nonumber \\rho _{M} (d^2) &=& \left\lbrace \begin{array}{ll}\Gamma - d^2, & \textrm {if} \quad d^2 < T_M \0, & \textrm {if} \quad d^2 \ge T_M \end{array}\right. \end{eqnarray} \]
\(d_{cr}^2,d_{rc}^2\) 是一個幀到另一個幀的傳遞誤差，\(T_M\) 是拒絕門限值，\(\Gamma\) 等於 \(T_H\) 這樣所有的模型在得分均勻。 最後選取score最高的一組。如果沒有模型被找到，那麽就重新回到步驟一（we restart the process again from step 1）。Model selection: 如果場景是近似於平面的，就可以用 homography 模型，否則選用 fundamental matrix 模型。在平面的情況下視差較低，使用 homography 重建的誤差較低。文章設計了一個式子：
\[ R_H=\frac{S_H}{S_H+S_F} \]
當 \(R_H>0.45\) 的時候選擇 homography 模型。Motion and structure from motion recovery: 一旦選好了模型，我們就可以獲得運動狀態。在 homography 中，從方法 23 中提取8種運動假設，這個方法通過測試來選擇有效的方案。如果測試失敗就會返回 1 中重新做計算，
\[ {\bf E}_{rc} = {\bf K}^{T} \, {\bf F}_{rc} \, {\bf K} \]Bundle adjustment: 使用 full BA 來初始化地圖。
5. Tracking
這一節詳細介紹線程通過相機執行每一幀的步驟。
A. ORB 提取
為了確保角點分布均勻，將一張圖片分成多個 cell ，每個 cell 提取至少五個角點。如果找不到足夠的角點，通過調整檢測器閾值來重新提取。如果某些 cell 不包含角點，每個 cell 的角點也要進行調整。方向和ORB描述符會做進一步的計算。
B. 從前面的幀初始化位姿
如果最後一幀的 tracking 成功，就是用勻速運動模型來預測位姿並找出上一幀中匹配的點。如果匹配不夠，那麽使用更加寬泛的距離去尋找對應的點進行相應的優化位姿。
C. 全局重定位的初始姿態估計
如果 tracking 失敗，就把當前幀轉為 bag of words 然後到數據庫中做匹配。我們計算與每個關鍵幀中與地圖點相關聯的ORB 特征，使用 PnP 算法來確定相機位姿，如果有足夠的 inlier ，就可以對丟失的幀做出位姿估計。
D. Track Local Map
一旦對相機位姿和特征匹配有了估計，就可以將地圖投影到幀中來尋找更多的關聯點。定義與當前幀有交集的集合\(\mathcal {K}_1\) 和 \(\mathcal {K}_2\) ，形成一個 covisibility graph 。在 \(\mathcal {K}_1\) 中與當前幀有最多的匹配點的幀叫做 \(\mathcal {K}_{ref}\) 。\(\mathcal {K}_1\) 和 \(\mathcal {K}_2\) 中每個可見的點雲將會如下操作：
- 把 x 映射到當前幀，如果超出邊界就丟棄；
- 計算當前視角 \(\bf v\) 和點雲中平均視角 \(\bf n\)，如果 \(\mathbf {v} \cdot \mathbf {n}<\textrm {cos}(60^{\circ})\) 就丟棄當前視角；
- 計算點雲到相機中心的距離，超出the scale invariance region 就丟棄；
- 計算幀的 scale，比例 \(d/d_{\min}\) ；
- 對比點雲中描述符 \(\bf D\) 和當前幀還沒有匹配的ORB特征，在scale，和靠近x的雲點作最優匹配。
相機位姿最後通過當前幀中獲得所有的點雲進行優化。
E. 新的關鍵幀選取
要盡可能快的插入關鍵幀，這樣可以在相機移動時 tracking 過程更加穩健。插入新的關鍵幀遵循：
- 上一次全局定位已經過去 20 幀；
- 局部地圖構建已經暫停，或者是距離上次插入關鍵幀超過20幀；
- 當前幀 track 出來 50個特征點；
- 當前幀跟蹤少於參考關鍵幀特征點的90%。
6. Local Mapping
A. 關鍵幀插入
首先更新 covisibility graph ，插入節點 \(K_i\) ，更新關鍵幀間具有相同點雲產生的edge。我們還要更新生成樹上Ki和其他關鍵幀的鏈接。然後，計算表示關鍵幀的詞袋，用於數據關聯來三角化新的雲點。
B. 點雲的去冗余
點雲必須通過三幀的嚴格測試，確保它們是可被 track 和沒有錯誤的 triangulate。一個點必須滿足兩個條件：
- Track 必須在超過25%預測可見的幀中找到該點；
- 特征點必須被至少三個關鍵幀察覺到。
C. 新點雲的創建
在 covisibility graph 中的關聯關鍵幀 $\mathcal {K}_c $ 三角化 ORB 可以創建點雲，ORB特征對三角化後，將要獲得新的雲點，這時要檢查兩個相機視圖的景深，視差，重映射誤差，和尺度一致性等。
D. 局部BA
局部優化當前處理的關鍵幀 \(K_i\) 、所有在 covisibility graph $\mathcal {K}_c $ 上關聯的幀和之前所有幀中的特征點。離群點會在優化的中間和最後階段被丟棄。
E. 局部關鍵幀剔除
為了使得重構更加簡潔，局部映射會嘗試探測冗余關鍵幀然後刪除它們。這會大有幫助，因為BA 的復雜度隨著關鍵幀的增加而增長。一個關鍵幀如果90%的特征點已經在至少三幀出現過，也會被剔除。
7. 閉環檢測
閉環檢測處理的是最近處理的關鍵幀，判斷它是否與之前的關鍵幀存在閉環。
A. 閉環候選檢測
首先，計算 \(K_i\) 和它在 covisibility graph 鄰近的關鍵幀的 bag of words vector 的相似度，保留最小得分 \(s_{\min}\) 。然後查詢數據庫，丟棄所有得分小於 \(s_{\min}\) 的關鍵幀，然後丟棄所有和 \(K_i\) 直接關聯的關鍵幀。接著從剩下的幀中選取候選項，候選項必須是連續的三幀以上的才能作為候選項。
B. 計算相似轉換
在單目 SLAM 中，可能有 7 個自由度發生漂移：三個平移分量，三個旋轉分量和 scale 因子。因此要計算 \(K_i\) 和回環幀 \(K_l\) 的相似度來計算環中的累計誤差。
首先計算當前關鍵幀和環候選關鍵幀中特征點的 ORB 對應關系，對應每個候選環，有 3D to 3D 的對應關系。對每個候選回環執行RANSAC叠代，，balabala
C. 閉環融合
第一步融合重復的點，在covisibility graph插入與 loop closure 有關的新的 edges。當前關鍵幀的位姿 \(\mathbf {T}_{iw}\) 通過相似變化 \(\mathbf {S}_{il}\) 修正，這個修正也會應用於所有的相鄰幀 \(K_i\) ，來執行 concatenating 變換，這樣環中所有的邊都可以對齊。
回環關鍵幀所有的特征點和它的鄰居映射到 \(K_i\) 中，通過映射在較小的區域內搜索它的近鄰和匹配。所有匹配和未離群的點都被融合起來，融合過程中所有的關鍵幀將會更新它們的邊緣，這些視圖內容相關的圖像創建的邊緣用於回環控制。
D. Essential Graph 優化
為了進行有效的閉環，我們在 Essential Graph 執行了一個優化，這樣可以將回環的誤差分散到圖像中，優化程序通過相似變換校正尺度偏移。優化過後每一個特征點都根據關鍵幀的校正進行變換。
ORB-SLAM: A Versatile and Accurate Monocular SLAM System 筆記（二）