PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features
摘要：為了解決基於慣性測量和視覺觀測的相機軌跡估計和構建結構三維（3D）地圖的問題，本文提出了點線視覺慣性里程計（PL-VIO），一種緊耦合利用點和線特征的單目視覺慣性里程計系統。與點特征相比，線提供了更多關於環境的幾何結構信息。為了獲得3D空間線的計算簡單性和表示緊湊性，采用普呂克坐標和線的正交表示。為了緊密有效地融合來自慣性測量單元（IMU）和視覺傳感器的信息，我們通過最小化成本函數來優化狀態，該成本函數將預先集成的IMU誤差項與點和線重新投影誤差項組合在滑動窗口中 優化框架。在公共數據集上評估的實驗表明，結合點和線特征的PL-VIO方法優於僅使用點特征的幾個最先進的VIO系統。
1. Introduction
近年來，定位和導航在廣泛的應用中引起了很多關注，特別是對於自動駕駛汽車，服務機器人和無人駕駛飛行器等。幾種類型的傳感器用於定位和導航，例如全球導航衛星系統（GNSS）[1]，激光雷達[2,3]，慣性測量單元（IMU）和攝像機[4,5]。然而，它們具有明顯的各自缺點：如果有清晰的天空視圖，GNSS僅提供可靠的定位信息[6]; 激光雷達遇到玻璃表面物體的反射問題[7]; 來自民用IMU的測量結果是有噪聲的，因此慣性導航系統可能由於誤差累積而快速漂移[8]; 單眼同時定位和制圖（SLAM）只能恢復到一定比例的運動軌跡，當攝像機快速移動或光照變化劇烈時，它往往會丟失[9-11]。因此，傳感器融合方法，特別是視覺慣性導航系統，已引起廣泛關注[12]。 來自IMU的加速度和角速度信息可以顯着改善單眼SLAM系統[13,14]。 此外，IMU和相機都重量輕且成本低，因此它們廣泛用於民用領域。
根據傳感器的直接或間接融合測量，視覺慣性里程計（VIO）系統可分為兩個主要流：松耦合和緊耦合方法。松散耦合方法[15,16]通過兩個估計器處理圖像和IMU測量，這兩個估計器分別估計相對運動並融合來自兩個估計器的估計以獲得最終結果。緊耦合方法[17,18]使用一個估計器通過直接融合來自攝像機和IMU的原始測量來找到最佳估計。 與松散耦合方法相比，緊耦合方法通常更准確和更健壯。 在本文中，提出的PL-VIO方法是一個緊耦合的VIO系統。 關於緊耦合VIO方法的相關工作可以通過測量模型中的線性化數量來分類[14]。這些基於擴展卡爾曼濾波器（EKF）的方法僅在更新步驟中處理一次測量，而批次非線性優化在優化步驟期間多次線性化。 基於濾波的方法[19,20]整合IMU測量以傳播/預測狀態，然后通過視覺測量更新/校正最新狀態。由於三維（3D）地標的坐標包括在狀態向量中，因此EKF的計算復雜度隨着地標的數量而呈二次方增加。 為了解決這個問題，Mourikis和Roumeliotis [21]提出了多狀態約束卡爾曼濾波器（MSCKF），它從狀態向量邊緣化了地標坐標。 該方法的缺點是用於更新狀態的界標測量需要移出攝像機的視野，這意味着並非所有當前的視覺測量都在過濾器中使用。 此外，線性化誤差使濾波器不一致[14]。
基於優化的方法通過最小化具有IMU測量殘差和視覺重投影殘差的聯合非線性成本函數來獲得最優估計。 因此，基於優化的方法可以在不同點重復狀態向量的線性化，以實現比基於過濾的方法更高的精度[14]。通過在幀之間積分IMU測量來計算IMU測量約束。 然而，標准IMU集成方法與第一幀的初始IMU體狀態緊密相關。 當估計的狀態改變時，需要重新計算所有集成的IMU測量值。 Lupton和Sukkarieh [22]提出了一種IMU預集成技術，可以避免這種重復集成。IMU預集成已廣泛應用於基於優化的VIO [18,23,24]。 Forster等[14]通過在流形上處理旋轉組而不是使用歐拉角來重新構造IMU預積分。 Liu等[13]提出了連續預積分方法。 盡管基於優化的方法已經實現了高精度，但是隨着越來越多的地標被添加到優化中，計算變得昂貴。OKVIS [18]使用先進先出滑動窗口方法通過邊緣化最舊狀態的測量來進行綁定計算。 Shen等[23]提出了雙向邊緣化，以選擇性地邊緣化身體狀態和地標。
盡管VIO領域取得了重大成就，但大多數VIO系統僅使用點要素作為視覺信息。 然而，無紋理環境中的點檢測和照明改變場景中的點跟蹤是具有挑戰性的[25,26]。相比之下，線段是這些場景中的合適替代解決方案。 此外，線段提供了比環境更多的環境結構信息[27]。 對於僅視覺SLAM，有幾種結合點和線特征的工作來估計相機運動[28,29]。 在SLAM系統中集成線要素的最簡單方法是使用兩個端點來表示線。 匹配來自不同視圖的線的端點是困難的。 此外，3D空間線僅具有四個自由度（DoF），而兩個3D端點引入六個參數，這導致過度參數化。Bartoli和Sturm [30]提出了正交表示，它使用三自由度旋轉矩陣和一個多自由度旋轉矩陣來更新優化期間的線參數。 正交表示已經用於一些立體視覺SLAM系統[27,31]。 對於VIO方法，Kottas和Roumeliotis [26]僅使用線路特征研究了VIO的可觀察性。 Kong等人[25]利用三焦幾何構建了一個結合點和線特征的立體聲VIO系統。 但是，這些工作涉及基於過濾的VIO。 在我們提出的PL-VIO方法中，我們將線要素集成到優化框架中，以實現比基於過濾的方法更高的精度。
為了構建結構3D地圖並獲得相機的運動，我們提出了PL-VIO系統，它通過聯合最小化IMU預積分約束以及滑動窗口中的點和線重新投影誤差來優化系統狀態。 與僅使用點特征的傳統方法相比，我們的方法利用附加線特征，旨在提高照明變化環境中的魯棒性和准確性。 我們的主要貢獻如下：
據我們所知，擬議的PL-VIO是第一個使用點和線作為地標的基於優化的單目VIO系統。
為了緊密有效地融合來自視覺和慣性傳感器的信息，我們引入了具有IMU預積分約束和點/線特征的滑動窗口模型。 為了在優化中緊湊地表示3D空間線，采用線的正交表示。導出關於IMU體狀態的所有誤差項的雅可比矩陣，以有效地求解滑動窗口優化。
我們在EuRoc數據集和PennCOSYVIO數據集上比較了所提出的PL-VIO與三種最先進的單眼VIO方法的性能，包括ROVIO [17]，OKVIS [18]和VINS-Mono [32]， 報告詳細的評估結果。
本文的其余部分安排如下。 首先，我們在第2節中描述了數學預備，然后在第3節中制定了基於滑動窗口的視覺 - 慣性融合方法。接下來，我們將在第4節中描述我們的PL-VIO系統和實現細節。第5節顯示實驗結果。 最后，第6節給出了結論和未來的潛在工作。
2. Mathematical Formulation
2.1. Notations
圖1顯示了視覺慣性傳感器，以及點和線特征的視覺觀察。我們將c i表示為時間t = i時的相機幀，並且b i同時表示為IMU體幀。
2.2. IMU Pre-Integration
一個6軸IMU，包括一個3軸加速度計和一個3軸陀螺儀，可以測量車體框架相對於慣性框架的加速度a和角速度ω[14]。原始測量ω和â是 受偏見和白噪聲的影響：
where ⊗ denotes the quaternion multiplication operation.
預積分模型（等式（6））源自連續時間並忽略偏差和噪聲。 在實踐中，IMU測量是從離散時間收集的，應考慮噪聲。 在這項工作中，我們使用中點集成來集成IMU測量。 使用離散時刻k和k + 1處的測量的IMU體傳播通過以下公式計算：
在實踐中，偏見變化緩慢。 我們用隨機游走噪聲模擬偏差：
2.3. Geometric Representation of Line
直線只有四個DoF。 因此，直線的緊湊參數化具有四個參數。 在我們的系統中，我們將3D空間中的直線視為無限直線，並采用兩個參數化作為3D線，如[27]中所述。由六個參數組成的Plücker線坐標由於其簡單性而用於變換和投影。 由四個參數組成的正交表示由於其緊湊性而用於優化。
2.3.1. Plücker Line Coordinates
2.3.2. Orthonormal Representation
由於3D空間線僅具有四個自由度，因此在優化期間，正交表示（U，W）∈SO（3）×SO（2）比Plücker坐標更合適。另外，正交表示和Plücker坐標可以相互轉換，這意味着我們可以在SLAM系統中采用它們用於不同的目的。 在本節中，我們將介紹正交表示的細節。 如圖2a所示，在3D線上定義坐標系。 歸一化法向量和歸一化方向向量是坐標系的兩個軸。 通過交叉其他兩個軸向量確定第三軸。 我們可以將線坐標和相機框架之間的旋轉矩陣定義為U：
3. Tightly-Coupled Visual–Inertial Fusion
在視覺SLAM中，通過最小化圖像平面中的重新投影誤差，使用束調整來優化相機姿勢和3D地圖點。 通過非線性優化進行的束調整可以視為因子圖[35]，如圖3a所示：圓形節點是需要優化的相機姿態或3D地標; 帶方框的邊表示視覺測量值作為節點之間的約束。 對於視覺慣性融合，我們還可以使用緊耦合的基於圖的框架來優化視覺慣性系統的所有狀態變量[23]。 如圖3b所示，因子圖不僅包含視覺測量，還將預先集成的IMU測量作為邊緣來約束連續的IMU體狀態。
3.1. Sliding Window Formulation
為了實現計算效率和高精度，我們使用滑動窗口公式進行因子圖優化。 在時間i的滑動窗口中的完整狀態變量被定義為：
其中x i由世界框架中的第i個IMU主體位置，速度和方向以及IMU主體框架中的偏差來描述。 下標n，m和o分別是身體狀態，點地標和線地標的起始索引。 N是滑動窗口中關鍵幀的數量。M和O分別是滑動窗口中所有關鍵幀觀察到的點標志和線標志的數量。 我們僅使用一個變量，即逆深度λk，從其第一個觀察到的關鍵幀參數化第k個點地標。 O l是世界框架w中第l行特征的標准正交表示。
我們通過最小化所有測量殘差的成本項之和來優化滑動窗口中的所有狀態變量：
我們使用Levenberg-Marquard算法來解決非線性優化問題（20）。通過從初始猜測X 0迭代更新，可以找到最優狀態估計X：
其中⊕是用於以增量δX更新參數的運算符。 對於位置，速度，偏差和反向深度，更新運算符和增量δ很容易定義：
然而，更新運算符和狀態態q的增量δ更復雜。四元數中使用四個參數來表示三自由度旋轉，因此它被過度參數化。旋轉的增量應該只是三維的。 與[18]類似，我們在切線空間中使用擾動δθ∈R3作為旋轉增量。 因此，可以通過四元數乘法更新旋轉q：
3.2. IMU Measurement Model
由於通過等式（10）計算的預積分IMU測量值是兩個連續關鍵幀b i和b j之間的約束因子，因此IMU測量殘差可以定義為：
其中[·] xyz提取四元數的實部，用於逼近三維旋轉誤差[18]。
在非線性優化期間，IMU測量殘差的雅可比矩陣相對於身體狀態x i和x j通過以下公式計算：
3.3. Point Feature Measurement Model
3.4. Line Feature Measurement Model
4. Monocular Visual Inertial Odometry with Point and Line Features
如圖4所示，建議的PL-VIO系統有兩個主要模塊：前端和后端。 前端模塊用於預處理來自IMU和攝像頭的測量。后端模塊用於估計和優化身體狀態。 我們將在以下小節中介紹細節。
4.1. Front End
前端從攝像機和IMU的原始測量值中提取信息。 通過每次新IMU測量的傳播來更新身體狀態，並且最新的身體狀態被用作滑動窗口優化中的初始值。 另外，新的IMU測量被預先集成以在優化期間約束連續的IMU身體狀態。
對於圖像處理，在兩個單獨的線程中檢測點和線特征。當新幀到來時，通過KLT光流算法[37]從前一幀到新幀跟蹤點特征。然后，我們使用RANSAC框架和基本矩陣測試來刪除異常值。如果離群值抑制后跟蹤點要素的數量小於閾值，則將添加由FAST檢測器[38]檢測到的新拐角特征。對於線特征，LSD檢測器[39]檢測新幀中的線段，並通過基於外觀的描述符LBD [40]與前一幀中的線段匹配。我們使用幾何約束來刪除線匹配的異常值。例如，兩條匹配線的中點之間的距離應不大於dist像素，並且角度差應不大於δth角度。在特征檢測和匹配之后，將線段的點特征和端點投影到歸一化圖像平面上。另外，如果跟蹤點特征的平均視差大於閾值，則選擇幀作為新關鍵幀。
4.2. Back End
在后端線程中，首先對點和線進行三角測量以構建重新投影殘差。為了獲得良好的地標估計，利用所有觀測值估計點特征的逆深度。 對於線三角測量，我們只在滑動窗口中選擇具有最遠空間距離的兩個幀來初始化Plücker坐標。
在我們獲得地圖點/線的初始估計以及從IMU測量預測的IMU體狀態之后，使用第3節中描述的滑動窗口優化來找到最佳狀態。 為了限制狀態向量X的大小，采用雙向邊緣化策略從滑動窗口中去除狀態[23]。當第二個最新的幀x n + N - 1是一個關鍵幀時，我們將所有測量結果邊緣化出最早的幀x n。 否則，如果第二個最新幀不是關鍵幀，我們將丟棄此幀中的可視測量值，並在預積分測量中保留其IMU測量值。 基於邊緣化測量獲得新的先驗信息，從被移除的狀態保留約束信息。
最后，我們剔除無效的地圖點和線。 如果點的反深度為負，我們將從地圖中刪除此點。 如果線的重新投影殘差超過閾值，則將從地圖中移除。
4.3. Implementation Details
5. Experimental Results
我們使用兩個公共基准數據集評估了我們的PL-VIO系統：EuRoc MAV數據集[44]和PennCOSYVIO數據集[45]。將PL-VIO方法的准確性與三種最先進的單眼數據進行比較 用於驗證PL-VIO方法優點的VIO方法：單眼模式下的ROVIO [17]和OKVIS [18]，以及沒有閉環的VINS-Mono [32] .ROVIO是一種基於擴展的緊密耦合形式的VIO 卡爾曼濾波器（EKF）。 它直接使用圖像中的強度誤差來找到更新步驟中的最佳狀態。 OKVIS是一種滑動窗口優化算法，具有單點或立體模式的點特征.VINS-Mono是一個完整的VIO-SLAM系統，采用點特征在滑動窗口中優化IMU體狀態，並執行循環閉合。 所有的實驗都在計算機上進行，配備Intel Core i7-6700HQ CPU，2.60 GHz，16 GB RAM和ROS Kinetic [46]。
5.1. EuRoc MAV Dataset
EuRoc微型飛行器（MAV）數據集由MAV在兩個室內場景中收集，其中包含來自20FPS的全局快門相機的立體圖像和200Hz的同步IMU測量[44]。 每個數據集提供由VICON運動捕捉系統給出的地面實況軌跡。 所有的外在和內在參數也在數據集中提供。在我們的實驗中，我們只使用左相機的圖像。
線要素的主要優點是它們提供了與環境有關的重要幾何結構信息。 作為一個例子，我們展示了由PL-VIO從圖5中的MH_05_difficult序列構建的重建地圖。圖5a-d中的四個圖像由機房大廳中的MAV捕獲，其顯示了房間的結構。 如圖5d所示，弱照明場景中的線段檢測比點特征檢測更穩健。從重建的3D圖可以看出環境的幾何形狀由線段描述，因此語義信息 可以從地圖中提取。 這對機器人導航很有用。
為了進行定量評估，我們將PL-VIO與三種最先進的單眼視覺慣性方法進行了比較：單眼模式下的ROVIO [17]，OKVIS [18]和沒有閉環的VINS-Mono [32]。 為了公平比較，使用了這些比較方法的作者提供的默認參數。 我們選擇絕對姿態誤差（APE）作為評估指標，直接比較估計與地面實況之間的軌跡誤差[47]。 開源軟件包evo（https://michaelgrupp.github.io/evo/）提供了一個易於使用的界面來評估測距和SLAM算法的軌跡。我們在本節中使用此工具來評估這些方法。 表1顯示了沿着所有軌跡的平移和旋轉的均方根誤差（RMSE），並且還提供了它們的直方圖，如圖6所示。
表1顯示，除了V1_01_easy之外，我們的PL-VIO共同優化了點和線，為旋轉的八個序列提供了最佳性能。 除了V1_01_easy，V1_02_medium和V1_03_difficult之外，我們的方法在六個平移序列上也表現最佳。但是，相對於最佳結果的差異僅在亞毫米級別。 表1中的結果表明，將線特征集成到VIO中可以提高運動估計的准確性。為了直觀地展示結果，PL-VIO和VINS-Mono估計的軌跡的幾個熱圖如圖7所示。顏色越紅，平移誤差越大。 比較三個軌跡，我們得出的結論是，當相機快速旋轉移動時，具有線條特征的PL-VIO比VINS-Mono產生更小的誤差。 此外，我們發現這些快速旋轉的序列引起觀察方向的大的變化，並且照明條件對於跟蹤點特征尤其具有挑戰性[25,26,28]。 如圖8所示，27對點（包括10個異常值）成功匹配，而33線成功匹配且所有匹配都正確。
此外，表2列出了不同方法的計算時間。基於濾波器的ROVIO的計算效率最高，而其精度最低。 提出的PL-VIO系統是最耗時的方法，但其計算時間主要受線路檢測和匹配步驟的限制。 在5.3節中，PL_VIO中不同模塊的計算時間在V1_03_difficult序列中獨立估算，並且發現PL-VIO系統的計算效率直接取決於線路檢測和匹配。
5.2. PennCOSYVIO Dataset
PennCOSYVIO數據集是最近的VIO基准測試，它使用從室外到室內的手持設備收集大型玻璃建築的同步數據（參見圖9）[45]。 挑戰因素包括光照變化，快速旋轉和重復結構。 提供了所有內在和外在參數以及地面實況軌跡。 我們使用VI傳感器收集的數據，也用於EuRoc MAV數據集。
我們將提出的PL-VIO與沒有閉環的VINS-Mono進行了比較。 為了公平地評估，相同的參數用於PL-VIO和VINS-Mono。 這里采用5.1節中使用的相同度量和評估工具來評估軌跡。 表3列出了結果。
此外，評估工具（https://daniilidis-group.github.io/penncosyvio/）也在PennCOSYVIO數據集中提供，並采用兩個指標，即APE和相對姿勢誤差（RPE）。 對於RPE，它通過將值除以路徑長度來表示百分比誤差[45] .PennCOSYVIO的創建者謹慎選擇評估參數，因此他們的工具適用於評估此數據集中的VIO方法。 因此，我們在實驗中采用了這種評估工具，結果列於表4中。
從表3和表4可以看出，PL-VIO獲得了旋轉部件的最佳性能。 由PennCOSYVIO工具評估的翻譯APE提供了更多細節。 與VINS-Mono相比，PL-VIO在y軸和z軸上產生較小的誤差，並且三個軸的誤差總和較小。 VINS-Mono僅在x軸上獲得了更好的性能。
5.3. Computing Time
最后，我們評估了在V1_02_medium序列上運行的PL-VIO的平均執行時間，因為該圖像序列是從典型的室內場景中收集的。 表5顯示了每個塊的執行時間。 我們可以看到，前端以11 Hz運行的線路檢測和匹配是效率方面的瓶頸。最先進的線路檢測和匹配方法，例如LSD和LBD的組合，對於VIO / SLAM系統來說並不令人滿意。 請注意，我們的方法與線特征檢測和匹配無關，因此提高其效率超出了本文的范圍。 后端的邊緣化是另一個耗時的部分。 我們觀察到邊緣化的低效率是由邊緣化特征時的填充引起的，這使得Hessian矩陣變得不那么稀疏矩陣。 通過在執行邊緣化時丟棄一些特征來維持稀疏的Hessian矩陣，可以解決這個問題[18]。
6. Conclusions
本文介紹了新穎的緊耦合單目視覺 - 慣性測距算法PL-VIO，該算法優化了具有點和線特征的滑動窗口中的系統狀態。提出的PL-VIO系統有兩個主要模塊：前端和前端后端。前端模塊用於傳播IMU體狀態，並檢測和匹配點/線特征。后端模塊用於估計和優化身體狀態。在后端模塊中，線標志被認為是無限3D空間線，並且其正交表示用於在優化期間緊湊地參數化。此外，為了有效地求解滑動窗口優化，詳細給出了誤差項的所有雅可比矩陣。我們還提供了所提出的PL-VIO的評估結果，與三種最先進的單眼VIO方法相比，包括ROVIO [17]，OKVIS [18]和VINS-Mono [32]對EuRoc數據集和PennCOSYVIO數據集。根據分析和結果，得出兩個進一步的結論如下：
具有線特征的重建的3D地圖可以提供關於環境的幾何信息，因此可以從地圖提取語義信息。 這對機器人導航很有用。
線要素可以提高平移和旋轉的系統精度，尤其是在照明變換場景中。 然而，線路檢測和匹配是耗時的並且成為系統效率的瓶頸。
在未來，我們計划通過在三維空間線之間引入結構約束來改進我們的系統，例如曼哈頓世界場景中的平行線或共面線[48]。 這些線之間的幾何約束有可能進一步提高定位精度並減少旋轉累積誤差。