RP-VIO: Robust Plane-based Visual-Inertial Odometry for Dynamic Environments

來源：Ram K, Kharyal C, Harithas S S, et al. RP-VIO: Robust Plane-based Visual-Inertial Odometry for Dynamic Environments[J]. IROS 2021

單位：印度海得拉巴機器人研究中心；

代碼開源：https://github.com/karnikram/rp-vio

針對問題：

面向動態(tài)場景的魯棒視慣融合里程計

提出方法：

充分利用場景中的平面約束，在初始化、滑窗優(yōu)化中對自身位姿進行優(yōu)化

達到效果：

相較于VINS-Mono、VINS-Mask，RP-VIO在論文自建數據集、VIODE、OpenLORIS-Scene、ADVIO數據集上實現了魯棒且準確的定位效果。

Abstract

VINS系統(tǒng)在實際部署中面臨著一個關鍵的挑戰(zhàn)：它們需要在高度動態(tài)的環(huán)境中可靠而魯棒地運行。目前最好解決方案根據物體的語義類別將動態(tài)物體作為外點剔除。這樣的方法無法擴展，因為它要求語義分類器包含所有可能移動的物體類別；這很難定義，更不用說部署了。另一方面，許多實際環(huán)境以平面的形式表現出強烈的結構規(guī)律性，如墻壁和地面。文章提出了RP-VIO，系統(tǒng)利用這些平面信息，在具有挑戰(zhàn)性的動態(tài)環(huán)境中提升了系統(tǒng)魯棒性和準確性。由于現有數據集的動態(tài)元素數量有限，文章還提出了一個高度動態(tài)、逼真的仿真數據集，以便更有效地評估VINS系統(tǒng)的能力。文章在這個數據集和來自標準數據集的三個不同序列（包括兩個實際場景的動態(tài)序列）上評估了文章的方法，相較于最先進的單目視覺里程計，系統(tǒng)表現出在魯棒性和準確性上得到顯著的改善。

Introduction

現有VIO系統(tǒng)具有一些局限性，首先除了需要精確同步和校準的額外硬件外而且該系統(tǒng)還需要進行足夠的旋轉和加速運動以保持重力和尺度的可觀察性。另一個重要的限制是他們在有多個獨立運動物體的動態(tài)環(huán)境中的表現?；镜亩嘁暯菐缀渭s束只適用于靜態(tài)點，當應用于動態(tài)點時就會導致錯誤。這個問題在單目VINS的初始化階段尤其重要，在這個階段，來自視覺SFM的姿態(tài)估計通常直接與IMU預積分結果測量一致，以初始化尺度和IMU參數。在這個階段，不正確的視覺姿態(tài)估計會導致完全的跟蹤失敗。

一個可行的方法是通過語義信息，直接識別場景中的靜態(tài)部分，以進行特征跟蹤。我們注意到，平面是日常人造環(huán)境中最豐富的靜態(tài)區(qū)域。重要的是，平面還提供了一個簡單的幾何形狀，可以進一步利用它來改善估計?；谶@一觀點，文章提出了RP-VIO，一個為動態(tài)環(huán)境定制的基于平面的單目視覺里程計。RP-VIO只使用場景中一個或多個平面的特征，由一個平面分割模型識別，并使用平面單應性進行運動估計。我們用我們提出的單應性約束增強了最先進的單目VIO系統(tǒng)，并在仿真以及實際場景數據集上顯著提高了性能。

Main Contributions

RP-VIO是一個在初始化和滑動窗口估計過程中只使用平面特征及其導出的單應性約束的單目VIO系統(tǒng)，以提高動態(tài)環(huán)境中的魯棒性和準確性。

構建了一個逼真的包含視覺以及IMU信息的數據集，與現有的數據集不同，它包含了整個序列（包括初始化）中的動態(tài)人物，并有足夠的IMU激勵。

在自建數據集上對所提出方法進行了廣泛的評估，一個來自最近發(fā)布的VIODE數據集的戶外模擬序列，以及來自OpenLORIS-Scene和ADVIO的兩個具有挑戰(zhàn)性的真實世界序列，使用了一個基于CNN的平面分割模型。

Method

我們提出的方法足夠通用，可以集成到任何視慣融合里程計或SLAM系統(tǒng)中，在這項工作中，我們以VINS-Mono為基礎。VINSMono是一個最先進的單目VIO，它是基于預集成的IMU測量和視覺特征的緊耦合的滑動窗口優(yōu)化。我們認為它是一個純粹的VIO系統(tǒng)，忽略了它的重新定位和閉環(huán)模塊。我們在其前端的基礎上，只檢測和跟蹤場景中的平面特征，并在初始化和優(yōu)化模塊中引入平面單應性約束。

1.符號定義

2.前端

我們的系統(tǒng)將灰度圖像、IMU測量值和平面分割掩碼作為輸入。這些平面分割遮罩是從一個基于CNN的模型中獲得的，我們在第三章E節(jié)中描述。我們在原始圖像上應用獲得的平面實例分割掩碼，只檢測和跟蹤屬于場景中（靜態(tài)）平面區(qū)域的特征，同時還保持每個被跟蹤特征屬于哪個平面的信息。為了避免檢測到可能屬于動態(tài)物體的面具邊緣的任何特征，我們對原始面具進行了侵蝕操作。此外，我們使用RANSAC對每個平面的特征進行單獨的平面同構模型，以拋棄任何異常值。這些離群值可能是由KLT光流算法的不正確匹配產生的特征，或者是不屬于更大的父平面的不準確的片段。圖像幀之間的原始IMU測量值被轉換為預集成測量值，具有足夠視差和特征軌跡的圖像幀被選作關鍵幀。

3.初始化

視覺-慣性滑窗優(yōu)化在給定狀態(tài)初值下通過迭代求解。為了獲得一個良好的初始估計，而不對起始配置做任何假設，使用了一個單獨的松耦合初始化程序，其中視覺測量和慣性測量被分別處理成各自的位姿估計，然后對齊求解其中未知參數。我們首先求解相機位姿、三維點和平面參數。從一個初始圖像幀的窗口中，選擇兩個具有足夠視差的基本幀。在它們之間的所有關聯上的特征中，我們選擇產生于場景中最大平面的匹配點對。我們將具有最多特征的平面確定為最大的平面。利用這些對應關系，我們使用RANSAC擬合了一個與兩個相機位姿和最大平面相關的平面單應性矩陣H。將單應性矩陣H歸一化后使用OpenCV中已經實現的Malis和Vargas的分析方法將其分解為旋轉、平移和平面法向量。然而，該方法最多可以返回四個不同的解，這些解必須進行條件判斷篩選。我們首先通過強制執(zhí)行正深度約束將這個解集減少到兩個，也就是說，所有的平面特征必須位于相機的前面。這個約束實現為： 

即使IMU預積分旋轉內部的陀螺儀偏置還沒有被估計出來，但因為其幅度通常很小，不會特別影響結果。分解出的位姿用來對兩個基本幀之間的對應特征點進行三角化，獲得一個初始點云。窗口內其余幀的位姿使用PnP進行估計。在此注意到由于兩個基準幀之間的位姿是以平面距離d為尺度的，所以三角化的點云和推導出的位姿也具有相同的尺度。所有的位姿隨后被送入一個BA優(yōu)化中，除了標準的3D-2D重投影殘差外，我們還包括以下由平面單應性產生的2D-2D重投影殘差。

用平面單應性矩陣從第一幀中映射其對應的圖像位置ul得到的。BA的輸出是按尺度（d）得到的相機位姿和三維點，以及平面法向量。這些經過BA調整的視覺估計仍然不足以初始化，因為還需要估計未知的尺度、重力矢量、速度和IMU的偏置。這些參數的估計方法與VINS-Mono中一致。視覺估計和IMU預積分之間尺度因子是到最大平面的距離d。加速計的偏置，通常需要更多的測量，為了將初始化時間限制在2秒以內，沒有進行解算。一旦所有的慣性量被估計出來，相機位姿和三維特征點被重新縮放為公制單位，世界坐標系被重新對齊，使其Z軸處于重力方向。對于場景中除最大平面外的其他平面，包括操作過程中可能新觀察到的平面，我們同樣計算它們各自的平面單應性矩陣并對其進行分解。但我們避免重新進行視覺BA，以及用IMU測量值重新調整它們的位姿，以及估計它們的尺度dp。我們直接將dp估計為每個分解的平移tp與對應公制平移tp的反比，這在之前已經用最大的平面和慣性測量進行了估計。有了這個結果，當前狀態(tài)下的所有視覺和慣性量都已進行求解，這些估計值被送入滑窗內作為優(yōu)化的初始種子點。

4.滑窗優(yōu)化

批量優(yōu)化位姿、地圖點、慣性測量和平面參數的整個歷史結果不能保證實時性，所以采用一個固定大小的滑窗進行優(yōu)化。優(yōu)化目標函數如下所示：

系統(tǒng)構建的因子圖如上圖所示，整個非線性目標函數通過使用Ceres Solver中實現的Dogleg算法和Dense-Schur線性求解器迭代求解。在優(yōu)化結束時，窗口向前移動一幀以納入最新幀。最新幀的狀態(tài)是通過傳播前一幀的慣性測量值進行初始化。如同VINS-Mono中的做法，丟棄的幀被邊緣化。而優(yōu)化后的平面參數并沒有被丟棄或邊緣化，而是在再次觀察到該平面時重新使用。

5.平面分割

為了從每個輸入的RGB圖像中分割出平面，文章基于Pane-Recover實施。他們的模型使用結構約束進行訓練，以同時預測平面分割掩碼和它們的三維參數，只有語義標簽，沒有明確的三維標注。該模型在單個Nvidia GTX Titan X (Maxwell)GPU上能以30 FPS運行適合于實時VIO。盡管他們的模型很有效，但我們在實驗中注意到，預測的片段往往不連續(xù)，單一的大平面被分割成多個獨立的平面。為了克服這個問題，我們引入了一個額外的損失函數，將之間相對方向小的平面約束為一個平面。

有了這個新增的損失函數，我們用他們提供的來自SYNTHIA的訓練數據重新訓練網絡，另外我們還對室內ScanNet數據集的兩個序列（00，01）進行訓練。為了進一步改善分割的邊界細節(jié)，我們采用隨機場模型來完善網絡的分割結果。下圖顯示了我們在評估中使用的一個未見過的實際場景的分割結果。

在這一節(jié)中，我們描述了如何從場景中檢測和跟蹤平面特征，如何利用IMU將平面單應性矩陣分解成各自的運動和平面估計，以及如何將平面參數作為附加約束引入初始化和滑窗優(yōu)化中。在下一節(jié)中，我們展示了這種方法在動態(tài)環(huán)境中的有效性。

Experiments

1.自建仿真數據集

RPVIO模擬數據集是我們生成仿真數據集，其中有準確的軌跡幀值，并且在整個序列中有足夠的IMU激勵。我們逐步在這些序列中加入動態(tài)元素，并使它們在序列的所有部分都可見，甚至在初始化期間也是如此。這使我們能夠分離出它們對整個系統(tǒng)精度的影響。四旋翼飛機被控制沿著半徑為15米的圓圈移動，同時沿著垂直方向的正弦波移動。沿著高度的正弦激勵是為了確保非恒定的加速度，并保持尺度的可觀性。我們進一步命令它在開始運動時，進行垂直加速，以幫助初始化。總的軌跡長度為200米，持續(xù)時間為80秒，最大速度為3米/秒。在四旋翼飛機形成的圓圈內，我們引入了正在進行重復性舞蹈運動的動態(tài)人物。我們在每個序列中逐步加入更多的動態(tài)角色，其他都是固定的，從靜態(tài)場景開始，到8個動態(tài)物體，總共記錄了6個序列。四旋翼飛機的偏航方向也是固定的，以保持相機指向圓心，這樣，在整個序列中，人物都在相機的視野范圍內。四旋翼飛機和人物是通過程序控制的，以確保他們的運動在所有序列中都是同步的。

VINS-Mono、Mask-VINS和RP-VIO在靜態(tài)和存在一個動態(tài)物體的序列上表現相似。因為靜態(tài)點的數量遠遠大于動態(tài)點的數量，RANSAC的效果與應用Mask相同。在存在兩個動態(tài)物體序列中，我們注意到VINS-Mono的精度比Mask-VINS和RP-VIO低得多，而Mask-VINS和RP-VIO的精度相似。當其中一個動態(tài)物體太相機時，VINS-Mono在初始化過程中積累了大部分的誤差，如下圖所示，

然而，在存在4(C4)個、6(C6)個和8(C8)個動態(tài)物體的序列中，VINS-Mono完全跟丟。在C4和C6中，Mask-VINS和RP-VIO仍然能夠成功跟蹤，但RP-VIO-Single是精度較高，這表現出了增加的單應性約束在改善魯棒性方面的作用。在C8序列中，我們的仍然能夠像其他序列一樣成功跟蹤，但Mask-VINS完全跟丟。這可能是因為場景非常雜亂，剩下的少數特征只來自初始化時的單一平面，這對VINS-Mono的基于基本矩陣的SfM初始化來說是一種退化的情況。在這個序列中，RP-VIO-Multi顯示出比RP-VIO-Single更好的準確性，這可能是RP-VIO-Multi相較于RP-VIO-Single有更多的觀測。

2.標準數據集

我們在VIODE、OpenLORIS-Scene、ADVIO三個序列上評估了我們系統(tǒng)的魯棒性。第一個數據集使用AirSim生成的，是一個有許多移動車輛的戶外城市環(huán)境中拍攝的，由一架正在進行包括急劇旋轉在內的無人機拍攝。我們使用他們提供的分割圖像，只沿著道路追蹤特征。第二個數據集是在現實世界的超市里從一個掃地機器人上采集的，其中包含許多動態(tài)的人物，如移動的人、手推車等。第三個數據集是在一個真實世界的地鐵站里用手持智能手機拍攝的，是三個序列中視覺上最有挑戰(zhàn)性的一個，它的視野很窄，動作很快，動態(tài)人物的形式是一輛移動的火車和移動的人。其中VIODE序列的總長度為166米，OpenLORIS序列為145米，而ADVIO序列為136米。

我們對三個序列使用我們方法的單平面版本進行測試。我們使用了與仿真實驗相同的特征參數，沒有進行任何調整。其與GT和其它方法相比得到的RMSE誤差如下表所示：

在這里，由于在所有三個序列中，所有被遮擋的特征主要來自一個平面，我們沒有與先前評估中使用的Mask-VINS進行比較，因為來自一個平面的特征對VINS-Mono初始化來說形成了一個退化情況。沒有一個現成的語義分類器可以準確地分割兩個序列中的所有動態(tài)物體，這也使得不能近似公平的對比。ADVIO序列中的圖像具有非常高的分辨率1280×720，采集頻率為60HZ，這導致了VINS前端的大量丟幀。出于這個原因，對這個序列的評估是在一個更強大的CPU上運行的，它有32GB內存和一個SSD。

討論：我們的方法在所有三個序列上都比VINS-Mono有明顯的提升。在OpenLORIS和VIODE序列中，我們的方法使用了比VINS-Mono更少的特征但得到了更高的準確性。這使我們相信，與跟蹤所有可能的特征相比，跟蹤少數穩(wěn)定的特征可能就足夠了，因為其中許多特征可能是有噪聲的。在這兩個真實世界的序列中，盡管使用的是沒有經過圖像訓練的通用平面檢測網絡，但該網絡和CRF能夠提供可靠的平面分割，足以讓我們的方法準確跟蹤。如果有訓練數據，我們希望能有更準確的分割和更好的整體軌跡估計。對于包含動態(tài)平面的場景，如車輛，必須訓練并使用特定的地面或墻壁表面分類器。但對特定的平面進行訓練仍然比對所有可能移動的物體類別進行語義分類器的訓練更可行。我們的方法應該被認為是對通用的基于點的系統(tǒng)的補充，而不是作為一個完全的替代。

Conclusion

我們提出了一個單目VIO系統(tǒng)，該系統(tǒng)只使用環(huán)境中的一個或多個平面以及它們的結構規(guī)律性來進行動態(tài)環(huán)境中的精確位姿估計。我們在不同的仿真和實際動態(tài)環(huán)境中驗證了其提升性能，同時在靜態(tài)場景中評估了它與baseline相同性能。對于現實世界的環(huán)境，只使用一個通用的平面分割模型，我們得到比最先進的單目VIO系統(tǒng)精度提高了45%。在我們與Mask-VINS的比較中，我們的方法比簡單的動態(tài)特征剔除方法取得了更好的準確性，這意味著了增加的結構約束在提高魯棒性方面的作用。這項工作的未來范圍是將其擴展到一個完整的SLAM系統(tǒng)中，以獲得干凈和一致的基于平面的地圖，沒有任何平面外的噪聲特征，這反過來可以用來進一步改善運動估計。此外，還可以研究來自分割模型預測的三維平面參數是否可以直接用于改進初始化。該方法還可以擴展到包括從分割的平面中產生的相應的線特征約束融合到系統(tǒng)中。

備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超干貨的3D視覺學習社區(qū)