＜論文閱讀＞用于 3D 點線和平面的視覺慣性裡程計架構 A Visual Inertial Odometry Framework for 3D Points, Lines and Planes

被疫情包圍了！在家多看看論文吧～

論文位址：A Visual Inertial Odometry Framework for 3D Points, Lines and Planes

【摘要】：恢複連續相機位姿之間的剛性配準是 3D 重建、SLAM 和視覺裡程計的核心。配準依賴于計算連續相機圖像中可區分的 2D 特征以确定特征對應的能力，這在特征較差的環境（即低紋理和/或低光環境）中非常具有挑戰性。在本文中，我們旨在解決視覺慣性裡程計（VIO）在特征較差的環境中恢複連續相機姿勢之間的剛性配準的挑戰。除了慣性傳感外，我們還為小型空中機器人配備了 RGBD 相機，并提出了一個架構，統一了 3D 幾何實體的結合：點、線和平面。跟蹤的 3D 幾何實體在擴充卡爾曼濾波架構中提供限制。我們表明，通過直接利用 3D 幾何實體，我們可以實作改進的配準。我們在不同的紋理較差的環境中展示了我們的方法，其中一些僅包含平坦的無紋理表面，基本上不提供用于跟蹤的 2D 特征。此外，我們通過将估計的姿态軌迹與從運動捕捉系統獲得的地面真實姿态軌迹進行比較來評估不同 3D 幾何實體的添加是否有助于改進姿态估計。我們考慮檢測 3D 點、線和平面的計算高效的方法，因為我們的目标是在小型移動機器人（例如無人機）上實施我們的方法。

一、引言

  裡程計和 SLAM 對于機器人在預先看不見的環境中導航至關重要。盡管有多種解決方案可用，但它們依賴于彩色相機圖像中的判别視覺特征來确定穩健的對應關系。紋理豐富的環境可以提供必要的視覺特征，并且許多遇到的環境都是紋理豐富的，例如戶外環境。然而，其他環境可能幾乎完全缺乏紋理，例如樓梯間和電梯井。此外，即使環境紋理豐富，低光照水準或霧等大氣條件也會對魯棒能力産生負面影響。

  如果相機圖像中沒有良好的視覺特征，機器人将不得不依賴其他傳感方式來提供 3D 測量以進行配準和姿勢估計。一種流行的模式是雷射雷達，它可以提供機器人周圍的 360°點雲。然而，雷射雷達傳感器既昂貴又笨重。在這種機器人是無人駕駛飛行器 (UAV)的情況下，雷射雷達傳感器的重量嚴重限制了其作為有效載荷的可行性。另一方面，RGBD 相機價格便宜且重量輕，是以是獲得 3D 測量結果的更理想選擇。盡管目前的 3D 重建和 SLAM 方法結合了 RGBD 相機并利用它們的深度資訊，但這些方法仍然依賴于彩色相機圖像的視覺特征。

  已經引入了額外的傳感器進一步協助 3D 重建和 SLAM，例如慣性測量單元 (IMU)。 IMU 的一個主要缺點是由于緩慢變化的偏置不穩定性和高頻噪聲的內建而導緻其測量值的漂移。航位推算主要依靠 IMU 進行配準，随着時間的推移會累積錯誤，并很大可能導緻重建和建圖配準錯誤。視覺慣性裡程計 (VIO) 架構已被引入以減少或消除這種漂移。除了 2D 點特征外，最近還考慮了用于 VIO 的 2D 線特征。假設環境可以（局部的）近似為曼哈頓世界（假設包含三個互相垂直的正交方向），該方法依靠跟蹤 2D 線作為相機圖像中的特征。

  我們提出了一個 VIO 架構，該架構可以處理紋理較差的環境并結合 3D 幾何實體：來自 RGBD 相機獲得的點雲的 3D 點、3D 線和 3D 平面。3D 點本身可能容易在點雲資料中出現噪聲，而對異常值具有魯棒性的 3D 線和平面的拟合，有助于降低對此類噪聲的敏感性。由于它依賴于深度測量而不是視覺特征，我們提出的方法的一個主要優點是它有可能在有霧、弱光條件或沒有視覺紋理的人造結構的環境中有效運作。此外，我們的架構與用于擷取點雲的特定 RGBD 相機無關。我們的重點是開發一種在能進入相對較小空間的無人機機器人上實施的方法。這限制了無人機的大小，進而限制了其有效載荷的大小和重量。

• 用于檢測和跟蹤 3D 點雲中的 3D 點、3D 線和 3D 平面的算法，包括從本地相機坐标系中跟蹤 3D 實體以估計自身在世界坐标系中位置的方法；
• 提出一個統一的 VIO 架構，将跟蹤的 3D 點、3D 線和 3D 平面作為幾何限制合并到擴充卡爾曼濾波 (EKF) 架構中。

三、帶有 3D 幾何實體的 VIO

  在我們的 VIO 架構中，将視覺資訊跟蹤的到的點、線對應關系，作為幾何限制合并到 EKF 中。在可能缺乏紋理或需要确定 2D 對應關系時，我們直接利用來自基本幾何的 3D 資訊。我們首先簡要概述使用 EKF 的符号和 VIO 架構，然後更詳細地描述我們提出的方法。

A. 符号和先決條件

  我們使用以下符号和定義。我們确定了幾個坐标空間：慣性傳感器的局部坐标系 I I I； RGBD相機的局部坐标系 C C C；和世界坐标系 W W W。從空間 A A A 到另一個空間 B B B 的（剛性）變換 M M M 用 M A B M^B_A MAB​ 表示。旋轉矩陣用 R R R 表示， R q R_q Rq​ 是通過操作 C ( q ‾ ) C(\overline{q}) C(q​)從四元數向量 q ‾ \overline{q} q​ 獲得的。符号 V × V\times V× 表示來自某個 三維向量 V V V 的 3 × 3 3 ×3 3×3 斜對稱矩陣。估計量用 x ^ \hat{x} x^ 符号裝飾，誤差量用 x ~ \tilde{x} x~ 裝飾，定義 x ~ = x − x ^ \tilde{x} = x −\hat{x} x~=x−x^ 用于某個量 x x x（例如，狀态向量）。

  VIO 的目标是在 EKF 估計架構中，根據視覺特征真實估計 IMU 狀态。時間 k k k 的 IMU 狀态定義為 X I M U k X_{IMU_k} XIMUk​​。其組成分别是世界和 IMU 坐标系之間的旋轉、陀螺儀偏差、世界坐标系中的 IMU 速度、加速度計偏差和世界坐标系中的 IMU 位置。在時間 k k k 的 EKF 狀态估計 X ^ k \hat{X}_k X^k​，用 N N N 個相機位姿增強 IMU 狀态。在 EKF 架構中，狀态估計在每個時間步更新如下： X ^ k + 1 = X ^ k + Δ X = X ^ k + K k r k \hat X_{k+1} = \hat X_k + ΔX = \hat X_k + K_kr_k X^k+1​=X^k​+ΔX=X^k​+Kk​rk​，其中 K k K_k Kk​ 表示卡爾曼增益矩陣， r k r_k rk​ 是殘差：

r k = H X ^ k + n o i s e (1) r_k = H \hat X_k + noise \tag{1} rk​=HX^k​+noise(1)這裡， H H H是測量雅可比矩陣，噪聲是零均值高斯白噪聲，與狀态向量 X X X不相關。

  測量模型依賴于從多個位姿觀察到靜态特征的概念，這對 IMU 的狀态估計施加了限制：姿勢估計應該與（跟蹤的）幾何實體的觀察結果一緻。在方程（1）中，目标是确定測量雅可比矩陣 H H H，我們現在将描述如何将 3D 點、3D 線和 3D 平面的觀測值合并到同一個VIO 架構中。

B. 3D 點

  給定一對連續的 RGBD 圖像，标記為源和目标，我們為點雲中的 3D 點計算 Fast Point Feature Histogram (FPFH) 特征。選擇FPFH是因為需要較高的計算效率，如果部署在實際無人機上，則需要實時計算。然後我們使用 FPFH 特征來确定源和目标中的 3D 點之間的對應關系。我們将在第 i i i 幀中觀察到的 3D 點 j j j 的位置表示為 z i ( j ) = C i p j = W C i R q ‾ ( W p j − W p C i ) z_i^{(j)} = ^{C_i}p_j = ^{C_i}_WR_{\overline q}(^Wp_j -^Wp_{C_i}) zi(j)​=Ci​pj​=WCi​​Rq​​(Wpj​−WpCi​​)。由于 W p j ^W p_j Wpj​ 是未知的，它是通過跟蹤 3D 點對應來估計的。我們将此估計定義為 W p ^ j ^W \hat p_j Wp^​j​ ，并可以估計測量值 z ^ i ( j ) = C i p ^ j = W C i R q ‾ ( W p ^ j − W p C i ) \hat z_i^{(j)} = ^{C_i}\hat p_j = ^{C_i}_WR_{\overline q}(^W\hat p_j -^Wp_{C_i}) z^i(j)​=Ci​p^​j​=WCi​​Rq​​(Wp^​j​−WpCi​​)，将測量誤差定義為： r i ( j ) = z i ( j ) − z ^ i ( j ) r_i^{(j)}=z_i^{(j)}-\hat z_i^{(j)} ri(j)​=zi(j)​−z^i(j)​。

  為了在 VIO 測量模型中加入 3D 點，我們更新關于狀态的測量 z i ( j ) z^{(j)}_i zi(j)​ 的雅可比矩陣，如下所示：

H X i ( j ) = [ 0 3 × 15 0 3 × 6 ⋯ ⌊ C i p ^ j × ⌋ − W C i R q ‾ ⋯ ] (2) H^{(j)}_{X_i} = \begin{bmatrix} 0_{3 \times 15}& 0_{3 \times 6} & \cdots & \lfloor ^{C_i}\hat p_j \times \rfloor & -^{C_i}_WR \overline q & \cdots\\ \end{bmatrix} \tag{2} HXi​(j)​=[03×15​​03×6​​⋯​⌊Ci​p^​j​×⌋​−WCi​​Rq​​⋯​](2)

  公式二中的斜對稱矩陣可以使用李群李代數得到，對于一個矩陣 R ∈ S O ( 3 ) R∈SO(3) R∈SO(3) 和一個函數 y = f ( R , x ) = R ⋅ x y=f(R,x) = R \cdot x y=f(R,x)=R⋅x，可以計算得到：

∂ y ∂ R = ∂ ∂ ω ∣ ω = 0 ( e x p ( ω ) ⋅ R ) ⋅ x = − ⌊ y ⌋ × {∂y \over ∂R} = {∂ \over ∂\omega}|_{\omega=0}(exp(\omega)\cdot R) \cdot x = -\lfloor y \rfloor_ \times ∂R∂y​=∂ω∂​∣ω=0​(exp(ω)⋅R)⋅x=−⌊y⌋×​其中 ω ω ω 表示 s o ( 3 ) \it {so(3)} so(3) 。

   z i ( j ) z^{(j)}_i zi(j)​ 關于特征位置的雅可比行列式是：

H i ( j ) = W C i R q ‾ (3) H^{(j)}_i = ^{C_i}_WR_{\overline q} \tag{3} Hi(j)​=WCi​​Rq​​(3) 然後堆疊 3D 點 j j j 的所有 M p t M_{pt} Mpt​ 觀測值的殘差。給定局部坐标系 C 中 3D 點 j j j 的跟蹤對應關系，可以通過取 W p ^ j i , i ∈ 1 , . . . , M p t ^W \hat p^i_j,i ∈ {1,...,M_{pt}} Wp^​ji​,i∈1,...,Mpt​ 的平均值輕松确定估計值 W p ^ j ^W\hat p_j Wp^​j​，，其中 W p ^ j i = ( W C i R q ‾ ) − 1 C i p j ^W \hat p^i_j = (^{C_i}_WR \overline q )^{−1C_i} p_j Wp^​ji​=(WCi​​Rq​)−1Ci​pj​。我們使用 RANSAC 來消除可能不正确的對應關系。

C. 3D 線

  除了 3D 點之外，我們的目标還包括 3D 線。在存在噪聲觀察的情況下，拟合一條線可能會減輕一些噪聲。此外，某些場景可能無法提供很多判别點特征，例如，如果幾何結構主要由平面組成。我們的目标是對于不同的場景可以穩定的獲得 3D 線，引入了各種 3D 線提取技術。為了避免昂貴的搜尋，我們隻識别那些點位于不同深度值之間的邊界上的線，這些線由深度圖确定。雖然我們可以使用為 3D 點對應計算的 FPFH 特征，但是對于每個 3D 點，我們計算了局部鄰域中的法線，并定義一個類似于 SIFT 特征的基于直方圖的特征。我們将具有相似特征的所有點組合在一起，并基于特征分析為它們拟合一條 3D 線 C i L j ^{C_i} L_j Ci​Lj​。

  作者用起點和終點去參數化一條線。在 3D 線的情況下，我們需要跟蹤這些 3D 點并在它們被相機視野遮擋或剪裁時更新它們。為了避免必須為在不同的觀察之間的同一條線确定對應錨點，我們僅通過其方向 d d d 參數化一條線。然後将一條線 j j j 轉換為：

z i ( j ) = C i L j = f ( w C i R q ‾ , W L j ) = w C i R q ‾ ⋅ W d j z^{(j)}_i = ^{C_i}L_j=f(^{C_i}_wR{\overline q},^WL_j)=^{C_i}_wR{\overline q}\cdot^Wd_j zi(j)​=Ci​Lj​=f(wCi​​Rq​,WLj​)=wCi​​Rq​⋅Wdj​

   z i ( j ) z^{(j)}_i zi(j)​關于狀态和特征位置的雅可比矩陣是：

H X i ( j ) = [ 0 3 × 15 0 3 × 6 ⋯ ⌊ C i d ^ j × ⌋ 0 3 × 3 ⋯ ] (4) H^{(j)}_{X_i} = \begin{bmatrix} 0_{3 \times 15}& 0_{3 \times 6} & \cdots & \lfloor ^{C_i}\hat d_j \times \rfloor & 0_{3 \times 3}& \cdots\\ \end{bmatrix} \tag{4} HXi​(j)​=[03×15​​03×6​​⋯​⌊Ci​d^j​×⌋​03×3​​⋯​](4) H i ( j ) = W C i R q ‾ (5) H_i^{(j)} = ^{C_i}_WR_{\overline q} \tag{5} Hi(j)​=WCi​​Rq​​(5)然後堆疊 3D 線 j j j 的所有 M l n M_{ln} Mln​ 觀測值的殘差。

  對于 3D 點，3D 線的估計 W d ^ j ^W{\hat d_j} Wd^j​ 可以通過對 3D 線 j j j 世界坐标系中的觀察進行求平均來得到。不同觀測之間的跟蹤線是通過比較世界坐标系中的線方向來執行的，并将方向在某個門檻值内的那些線進行比對。然而，這可能潛在地将具有相似方向但在不同 3D 位置的線進行比對。為了防止這種情況，對于給定的線 L j L_j Lj​，我們定義其 3D 點為 P L j = { p 1 , ⋯   , p m } L j P_{L_j}=\lbrace p_1,\cdots,p_m \rbrace_{L_j} PLj​​={p1​,⋯,pm​}Lj​​，首先一條線 P L j < τ P_{L_j} < τ PLj​​<τ，其中 τ τ τ 是被确定丢棄的内點數的門檻值，因為我們憑經驗發現它們不能穩健比對。其次，對于兩條線 L j L_j Lj​、 L k L_k Lk​，我們可以使用關聯的 P L j P_{Lj} PLj​ 和 P L k P_{L_k} PLk​​ 來計算歐幾裡得距離，如果距離在某個門檻值内，則将線 L j L_j Lj​、 L k L_k Lk​ 标記為對于，否則标記為不對應。

D. 3D 平面

  最後，我們還考慮了 VIO 的 3D 平面實體。由于點和線的噪聲，平面可以進一步減輕不确定性。在這個架構中對 3D 平面的處理遵循線的處理。線和平面之間的主要差別在于我們通過它們的法線方向參數化平面。

   z i ( j ) z^{(j)}_i zi(j)​ 關于狀态和特征位置的雅可比矩陣現在是：

H X i ( j ) = [ 0 3 × 15 0 3 × 6 ⋯ ⌊ C i n ^ j × ⌋ 0 3 × 3 ⋯ ] (6) H^{(j)}_{X_i} = \begin{bmatrix} 0_{3 \times 15}& 0_{3 \times 6} & \cdots & \lfloor ^{C_i}\hat n_j \times \rfloor & 0_{3 \times 3}& \cdots\\ \end{bmatrix} \tag{6} HXi​(j)​=[03×15​​03×6​​⋯​⌊Ci​n^j​×⌋​03×3​​⋯​](6) H i ( j ) = W C i R q ‾ (5) H_i^{(j)} = ^{C_i}_WR_{\overline q} \tag{5} Hi(j)​=WCi​​Rq​​(5)其中 n n n 表示平面法線。然後堆疊 3D 平面 j j j 的所有 M p l M_{pl} Mpl​ 觀測值的殘差。

  跟蹤 3D 平面與 3D 線相同，包括計算組成平面的 3D 點。出于與線相同的穩健性原因（見上文），組成點數量低于門檻值的平面被丢棄。

E. 降維

  對于世界坐标系下的特征 W f ^Wf Wf，即 3D 點、線或平面，我們有 r ( j ) ≈ H X ( j ) X ~ + H ( j ) W f ~ + n o i s e r^{(j)} \approx H_X^{(j)}\tilde X+H^{(j)} {^W} \tilde f+noise r(j)≈HX(j)​X~+H(j)Wf~​+noise。為了以等式（1）的方式規定殘差，我們執行零空間投影， 得到一個特征 j j j 的堆積誤差 r o ( j ) r_o^{(j)} ro(j)​ 和雅可比矩陣 H o ( j ) H_o^{(j)} Ho(j)​。在零空間投影之前，Jacobian 是一個 ( 3 M ( j ) × 3 ) (3M^{(j)} × 3) (3M(j)×3) 維矩陣，在投影之後，它的次元是 ( ( 3 M ( j ) − 3 ) × 3 ) ((3M^{(j)}-3) × 3) ((3M(j)−3)×3)，這裡 M ( j ) M^{(j)} M(j) 表示實體 j j j 的觀察次數：點、線或平面。

  如果我們假設 ∀ j ∀j ∀j， M ( j ) = M = M p t = M l n = M p l M^{(j)} = M = M_{pt} = M_{ln} = M_{pl} M(j)=M=Mpt​=Mln​=Mpl​，那麼給定 N p t N_{pt} Npt​個 3D點， N l n N_{ln} Nln​個 3D線和 N p l N_{pl} Npl​個 3D平面。通過組合所有實體，堆疊得到的雅可比矩陣是 ( ( N p t + N l n + N p l ) ( 3 M − 3 ) × 3 ) ((N_{pt}+N_{ln}+N_{pl})(3M−3)×3) ((Npt​+Nln​+Npl​)(3M−3)×3) 維的。很明顯，通過 QR 分解的降維對于降低計算複雜度是必不可少的。

F. EKF 更新

  我們現在使用有用的資訊來計算所需的卡爾曼增益以執行 EKF 更新，即确定更新的狀态和協方差估計。除了增加 EKF 狀态外，新觀測值的到來還增加了 EKF 更新所需的協方差矩陣。

五、結論

  我們提出了一個統一的架構，将 3D 點、線和平面用于視覺慣性裡程計。我們的方法适用于缺乏計算 2D 對應關系的弱紋理的環境，這是大多數先前方法所依賴的。我們的實驗表明，通過結合所有三個基元，我們可以提高後續幀之間的配準精度。使用 RGBD 相機擷取的深度或點雲會受到噪聲的影響，跟蹤 3D 點可能對這種噪聲很敏感。通過額外估計 3D 線和 3D 平面，可以減輕這種噪聲。為了避免不得不估計 3D 線和平面的所謂錨點，我們隻考慮這些實體的方向。是以，如果我們不能穩健地跟蹤 3D 點，我們需要不同方向的 3D 線和平面來限制所有三個次元。在某些環境下，可能沒有足夠的資訊來估計不同方向的 3D 線和平面。在這種情況下，我們總是可以依靠 IMU 進行航位推算，直到再次滿足必要條件，但會産生一些漂移。對于未來的工作，我們希望在自主無人機上實作我們的架構，并進一步證明它在紋理較差的環境中的有用性。