OKVIS論文翻譯： Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization

論文翻譯下載連結（帶圖）：http://download.csdn.net/detail/u014679795/9762881

論文原文：http://download.csdn.net/detail/u014679795/9762879

Keyframe-BasedVisual-Inertial SLAM Using Nonlinear Optimization

-----------------------------------------------------------------------------------------------------------------------------------------------------------

摘要 - 視覺和慣性感測器的融合在機器人技術中已經變得流行，因為兩種感測模態的互補性質。雖然目前大多數融合策略依賴於過濾方案，但是視覺機器人社群最近已經轉向用於諸如可視同步定位和對映（SLAM）之類的任務的非線性優化方法，隨後發現這種方法在效能質量方面具有顯著的優點和計算複雜性。遵循這一趨勢，我們提出一種新穎的方法，將視覺測量與來自慣性測量單元（IMU）的讀數緊密整合在SLAM中。IMU誤差項以完全概率的方式與地標再投影誤差整合，導致聯合非線性成本函式被優化。使用“關鍵幀”的強大概念，我們部分地將舊狀態邊緣化以維持有界大小的優化視窗，從而確保實時操作。與只有視覺慣性和鬆散耦合的視覺慣性演算法相比，我們的實驗證實了緊密融合在精度和魯棒性方面的好處。

I.    引言

結合視覺和慣性測量長期以來一直是解決常見的機器人技術任務，如運動估計，視覺測距和SLAM的流行手段。 影像中捕獲的場景的豐富表示，以及存在於典型IMU中的陀螺儀和加速度計的精確短期估計已經被認為彼此互補，在空中[6,20]和汽車[14] ]導航。 此外，隨著大多數智慧手機中這些感測器的可用性，對於視覺慣性SLAM的有效解決方案存在極大的興趣和研究活動。

歷史上，已經通過濾波處理了視覺慣性姿態估計問題，其中傳播IMU測量並且使用關鍵點測量來形成更新。 Mourikis和Roumeliotis [14]提出了一種基於EKF的使用單目視覺的實時融合，Jones和Soatto [8]在長的室外軌跡上提供單視覺慣性過濾結果，包括IMU到攝像機校準和環路閉合。這兩個作品表現出令人印象深刻的誤差低於0.5％的旅行距離。凱利和Sukhatme [9]提供校準結果和在基於過濾的visionIMU融合的上下文中的可觀察性的研究。視覺慣性估計問題是偏航和位置的全域性不可觀察性，以及相對於初始參考姿態的不確定性的增長;這對通常依賴於某種形式的線性化的濾波方法提出了挑戰。

在[18]中表明，在純視覺SLAM基於優化的方法提供了更好的準確性相同的計算工作，和過濾方法相比。保持相對稀疏的關鍵幀及其相關聯的地標的圖經歷非線性優化，因此已經非常受歡迎。

在文獻中發現的視覺 - 慣性融合方法可以分類為遵循兩種方法。在鬆耦合系統中， [10]，IMU測量結合作為獨立的傾斜儀和相對偏航測量到立體視覺優化。 Weiss等人[20]使用僅視覺姿態估計作為具有間接IMU傳播的EKF的更新。在[15,7]中，相對立體姿態估計被整合到包含慣性項和絕對GPS測量的因子圖中。這種方法限制了複雜性，但忽略了不同感測器的內部狀態之間的相關性。相反，緊耦合方法聯合估計所有感測器狀態。為了易於處理和作為過濾的替代，Dong-Si和Mourikis [2]提出了一種固定滯後平滑器，其中連續機器人姿勢和相關狀態的視窗維持，邊緣化狀態（在[19]之後），超出範圍。類似的方法，但沒有慣性術語和行星著陸的情況下使用[16]。

為了穩健和準確的視覺慣性SLAM，我們倡導緊密耦合融合，以最大限度地利用感測線索和非線性估計，而不是濾波，以減少由於線性化的次優性。我們的方法靈感來自[17]，其中建議在批量優化的SLAM中使用IMU誤差項（儘管僅在初始化期間）。 我們的方法與[2]中提出的固定滯後平滑器密切相關，因為它在單個成本函式中結合慣性項和再投影誤差，並且舊狀態被邊緣化以便限制複雜性。

關於這些作品，我們看到三個貢獻：

1）我們採用關鍵幀範例進行無漂移估計，當慢或完全沒有運動存在：而不是使用時間連續姿勢的優化視窗，我們保持關鍵幀可能在時間間隔任意遠，保持視覺約束同時仍然尊重IMU術語。 我們的關鍵幀的相對不確定性的公式允許建立一個姿態圖，而不表示全域性姿態不確定性，從RSLAM的靈感[13]。

2）我們提供IMU誤差項的完全概率推導，包括相關的連續影像幀的相應資訊矩陣，而沒有以IMU速率明確引入狀態。

3）在系統級，我們開發了用於精確實時SLAM的硬體和演算法，包括使用慣性提示的魯棒關鍵點匹配和異常值拒絕

在本文的剩餘部分中，我們在II-B中引入了批視覺SLAM中的慣性誤差項，隨後概述了II-C中的實時立體聲影像處理和關鍵幀選擇以及II-D中的邊緣化形式。 最後，我們展示了我們的立體視覺和IMU感測器室內和室外在III中獲得的結果。

II  緊耦合的視覺-IMU融合

在視覺SLAM中，公式化非線性優化，通過最小化在相機幀中觀察到的地標的再投影誤差來找到相機姿勢和界標位置。圖2示出了相應的圖表示：其將測量顯示為具有方框的邊和估計的量作為圓節點。 一旦引入慣性測量，它們不僅在連續姿勢之間產生時間約束，而且在加速度計和陀螺儀的連續速度和IMU偏差估計之間產生機器人狀態向量增加的時間約束。 在本節中，我們提出了將慣性測量結合到批量視覺SLAM中的方法。

A.符號和定義

1）符號：我們在整個工作中採用以下符號：表示參考幀A; 在其中向量被寫為pA或任選地作為pBCA，其中B和C分別作為起點和終點。 幀之間的變換由齊次變換矩陣T AB表示，變換矩陣T AB將均勻點的座標表示從到 ，其旋轉矩陣部分寫為CAB; 相應的四元數被寫為 和η表示虛部和實部。 我們採用Barfoot等人介紹的符號。 [1]：關於四元數乘法qAC =qAB⊗qBC，我們引入左手側複合運算子（.）+和右手側運算子（.）⊕，使得qAC = qAB + qBC =qBC⊕qAB。

2）幀：使用圖3中示意性地描繪的立體相機/ IMU設定來評估所提出的方法的效能。在被跟蹤物體內部，相對於慣性幀，我們區分相機幀和IMU感測器幀，

3）狀態：要估計的變數包括影像時間（索引k）xk R和地標xLc處的機器人狀態。xR表示在慣性幀pW SW中的機器人位置，身體旋轉四元數qW S，在慣性 幀的速度，以及陀螺儀的偏置bg和加速度計的偏置ba。 因此，xR被寫為：

此外，我們使用分割成姿態狀態和速度/偏置狀態。 地標以齊次座標表示，如[3]中所示，以便允許接近和非常遠的地標的無縫整合：

我們使用狀態歧管的切線空間g中的擾動，並採用組運算元，指數exp和對數log。 現在，我們可以定義擾動圍繞估計。 我們使用最小座標表示法。 雙射對映Φ從最小座標變換到切線空間：

具體來說，我們使用最小（3D）軸角擾動旋轉，其可以通過指數圖轉換成其等效四元數δq：

因此，使用組運算元⊗，我們寫。我們獲得最小機器人誤差狀態向量

類似於機器人狀態分解xT和xsb，我們使用姿態誤差狀態和速度/偏差誤差狀態。

 我們將齊次地標視為具有最小擾動的（非單位）四元數δβ，因此

B.具有慣性條件的批視覺SLAM

我們尋求制定視覺慣性定位和對映問題，作為包含（加權）重投影誤差er和來自IMU的時間誤差項es的成本函式J（x）的一個聯合優化：

其中i是元件的相機索引，k表示相機幀索引，j表示地標索引。 在第k幀和第i個相機中可見的標誌的索引被寫為集合J（i， k）。 此外，表示相應地標測量的資訊矩陣，是第k個IMU誤差的資訊。

固有地，純視覺SLAM具有需要在優化期間保持固定的6自由度（DoF），即， 絕對姿勢。 組合的視覺慣性問題僅具有4個自由度，因為重力使得兩個旋轉自由度可觀察到。這使固定複雜化。 我們想要凍結圍繞重力方向（世界z軸）的偏航，以及典型地第一姿態（index k1）的位置。 因此，除了將位置變化設定為零，，我們還假設

在下面，我們將呈現（標準）再投影誤差公式。 之後，給出了IMU運動學與偏差項建模相結合的概述，基於此IMU誤差項。

1）重投影誤差公式：我們使用相當標準的重投影誤差公式，適用於來自Furgale的小修改[3]：

因此，hi（·）表示相機投影模型，表示測量影像座標。 關於最小擾動的誤差Jacobian直接來自Furgale [3]。

2）IMU運動學：在假定地球旋轉的測量效應小於陀螺儀精度的假設下，我們可以將IMU運動學與簡單動態偏差模型結合起來：

其中都是不相關的零均值高斯白噪聲過程。 是加速度計測量值和地球重力加速度向量。 與被建模為隨機遊走的陀螺偏差相反，我們使用時間常數τ> 0來將加速度計偏差建模為有界隨機遊走。 矩陣Ω由估計的角速率形成，用陀螺測量資料

線性化誤差動力學採取形式

其中G是直接匯出和：

（.）×表示與向量關聯的斜對稱叉乘矩陣。

注意，可以以與在經典EKF濾波中用於平均值和協方差（PR，在最小座標中）的相同的方式使用等式（7）和（10）。對於實際實現，需要這些方程的離散時間版本，其中索引p表示第p個IMU測量。 對於計算複雜性的考慮，我們選擇使用簡單的Euler-Forward方法在時間差Δt上積分。 類似地，我們獲得離散時間誤差狀態過渡矩陣

這導致協方差傳播方程：

其中包含各個處理的所有噪聲密度

3）IMU測量誤差項的公式：圖4示出了在時間步驟k和k + 1進行的相機測量的測量速率的差異，以及通常不與相機測量同步的較快的IMU測量。

我們需要IMU誤差項是步驟k和k + 1處的機器人狀態的函式以及這些時間例項之間的所有IMU測量（包括加速度計和陀螺儀讀數）總結為。 因此，我們必須假設在相機測量k和k + 1的給定機器人狀態下的近似正常條件概率密度f：

對於狀態預測具有相關聯的條件協方差，IMU預測誤差項現在可寫為：

這只是基於先前狀態的預測和實際狀態之間的差異 - 除了方向，其中我們使用簡單的乘法最小誤差。

接下來，在應用誤差傳播定律時，找到相關聯的資訊矩陣為：

雅可比是直接獲得但不平凡，因為旋轉誤差一般將是非零的。

最後，關於和的雅可比矩陣將需要用於優化問題的有效解。儘管關於的微分是直接的（但不是微不足道的），但是一些注意力被給予另一個雅各比。 回想通過迭代地應用預測來計算IMU誤差項（14）。 相對於狀態的微分因此導致應用鏈式規則，產生

因此，表示特定離散步長的時間戳，並且pk表示在獲取相機幀k之後的第一IMU取樣索引。

C.關鍵點匹配和關鍵幀選擇

我們的處理流水線採用定製的多尺度SSE優化的Harris角點檢測結合BRISK描述子提取[12]。 檢測器通過逐漸抑制具有較弱分數的角落來強制影像中的關鍵點均勻分佈，因為它們在到較強角落的小距離處被檢測到。描述子被提取沿著重力方向（投影到影像中），其由於緊密的IMU融合而是可觀察的

最初，關鍵點被立體三角化並插入到區域性對映中。我們對所有地圖地標執行暴力匹配;通過使用通過IMU積分獲得的（不確定）姿態預測，通過在影像座標中應用卡方檢驗簡單地執行異常值拒絕。沒有昂貴的RANSAC步驟 - 緊密的IMU參與的另一個優點。對於後續優化，保持相機幀的有界集合，即具有在該時刻拍攝的相關影像的姿態;在這些影像中可見的所有地標都儲存在本地地圖中。如圖5所示，我們區分兩種型別的幀：我們引入包括當前幀的S個最近幀的時間視窗;並且我們使用可能在過去遠的N個關鍵幀。對於關鍵幀選擇，我們使用一個簡單的啟發式：如果匹配點跨越的影像區域與所有檢測到的點跨越的區域之間的比率低於50到60％，則幀被標記為關鍵幀。

D.部分邊緣化

非線性時間約束可以駐留在的有界優化視窗中是不明顯的，視窗中包含可能在時間上任意間隔開的關鍵幀。 在下面，我們首先提供邊緣化的數學基礎，即消除非線性優化中的狀態，並將其應用於視覺慣性SLAM

1.非線性優化邊際化的數學公式：高斯 - 牛頓方程組由所有的誤差項，雅可比和資訊構成：形式Hδx = b。 讓我們考慮要被邊緣化的一組狀態，與誤差項和剩餘狀態集合相關的所有狀態的集合。 由於條件獨立性，我們可以簡化邊緣化步驟，只將其應用於子問題：

Schur-Complement運算的應用產生：

其中和是和的非線性函式。

（18）中的方程描述了邊緣化的單一步驟。 在我們的基於關鍵幀的方法中，必須重複應用邊緣化步驟，並將結果資訊作為優先順序的先驗，因為我們的狀態估計繼續改變。因此，我們固定x0周圍的線性化點，邊緣化時x的值。 有限偏差表示在邊緣化之後發生的狀態更新，其中x是我們對x的當前估計。 換句話說，x被組成為

這個通用的公式允許我們將最小座標的先驗資訊應用到我們的任何狀態變數 - 包括單位長度四元數。 引入Δx允許右手側近似（到第一階）as

現在我們可以表示高斯 - 牛頓系統（17）：

在該形式中，右側（18）變為

在邊緣化節點包括無限遠（或足夠接近無限遠）的地標或來自單個姿勢的僅在一個攝像機中可見的地標的情況下，與那些地標相關聯的Hessian塊將（數字地）為秩不足。 因此，我們採用偽逆，其為提供瞭解，給定，零分量進入零空間方向。

上述公式為邊緣化xμ以及剩餘狀態xλ的狀態引入了固定線性化點。 這也將被用作涉及這些狀態的術語的所有未來線性化的參考點。在應用（18）之後，我們可以去除消耗的非線性項，並將邊緣化的和作為加數，以構建整個高斯 - 牛頓系統。對正方形誤差的貢獻可以寫為

2）邊緣化應用於基於關鍵幀的視覺慣性SLAM：

最初邊緣化誤差項由前N + 1幀構成， ，如圖6中以圖形方式視覺化.N個第一幀將全部被解釋為關鍵幀，並且邊緣化步驟包括消除相應的速度和偏置狀態

當將新幀（當前幀，索引c）插入到優化視窗中時，我們應用邊緣化操作。 在時間視窗中最舊的幀不是關鍵幀的情況下，我們將丟棄其所有的界標測量，然後將其與最舊的速度和偏置狀態一起邊緣化。圖7示出了該過程。 下降的地標測量是次優的;

然而，它保持稀疏問題的快速解決方案。 具有關鍵幀的視覺SLAM成功地類似地進行，丟棄具有其地標測量的整個幀。

在是關鍵幀的情況下，簡單地丟棄所有關鍵點測量的資訊丟失將更顯著：在共同界標觀測中編碼的最早的兩個關鍵幀之間的所有相對姿勢資訊將丟失。因此，我們另外將在中可見但在最近的關鍵幀中不可見的界標邊緣化。圖8圖形地描繪了該過程。 該問題的稀疏性再次得到保留。

IV  結論

本文提出了一種將慣性測量緊密整合到基於關鍵幀的可視SLAM中的方法。 非線性優化中的誤差項的組合由可用於關鍵點檢測和IMU讀數的誤差統計激發，因此取代了對任何調諧引數的需要。使用提出的方法，我們獲得重力方向的全域性一致性和使用IMU運動學運動模型的魯棒離群值排除。 同時，獲得了基於關鍵幀的非線性優化的所有好處，例如靜止姿態保持。使用立體攝像機和IMU感測器獲得的結果證明了所提出的框架的實時操作，同時在僅視覺或鬆散耦合方法上表現出增加的精度和魯棒性。

--------------------- 

轉載原文連結：https://blog.csdn.net/u014679795/article/details/56838526