Visual-Inertial Monocular SLAM With Map Reuse總結

王東曉 1900880

摘要

本文針對現(xiàn)在很多視慣融合的系統(tǒng)沒有閉環(huán)檢測導致位姿估計具有很大的漂移的問題，提出了基于視覺和慣導緊耦合的同時定位和建圖系統(tǒng)，這個系統(tǒng)不僅有閉環(huán)檢測，而且可以在已經(jīng)建圖的區(qū)域實現(xiàn)無偏的位姿估計。針對于單目具有尺度不確定性的問題，本文提出了新穎的IMU初始化的方法來獲得場景的尺度和重力的方向。該系統(tǒng)在micro-aerial數(shù)據(jù)集上的十一個序列進行了測試，得到了小于尺度1%的尺度漂移和厘米級的定位精度。通過這篇論文的學習和分析，知道了如何搭建緊耦合的視慣SLAM系統(tǒng)；學習了一種IMU初始化的方法；為進一步提升ORB-SLAM2的定位和建圖精度提供了思路和方法。

1. 系統(tǒng)簡介

本文提出了一種緊耦合VI-SLAM，它具有回環(huán)檢測，地圖重用等特性。由于和ORB-SLAM^[1]^[2]具有血緣關系，因此以下稱之為VI-ORB-SLAM簡稱為VI-ORB。同時VI-ORB系統(tǒng)具有新穎的初始化方法，來計算尺度、重力方向、速度、加速度計和陀螺儀的偏置（下稱biases）。

<center> 圖1. 在EuRoC[8]數(shù)據(jù)集的V1_02_medium上VI-ORB-SLAM估計出的地圖。頂視是使用估計出來的重力方向，綠色線連接著共視100個點以上的關鍵幀，這證明系統(tǒng)重用地圖的能力，使得當重新觀測相同地方時具有零漂移定位能力。</center>

2. 算法解析

2.1 視覺和慣導融合基礎

2.1.1 視覺信息

對于視覺信息流，即是圖像幀，使用小孔相機模型，相機投影函數(shù) $\pi : \mathbb R ^3 \to \Omega$ ，它將相機坐標系 $\mathrm C$ 下的3D點 $\mathbf {X _C} \in \mathbb R^3$ 變換到圖像平面的2D點 $\mathbf {x_c} \in \Omega \subset \mathbb R^2$ ，如公式（1）所示：
$\pi\left(\mathbf{X}_{\mathrm{C}}\right)=\left[\begin{array}{c} {f_{u} \frac{X_{\mathrm{C}}}{Z_{\mathrm{C}}}+c_{u}} \\ {f_{v} \frac{Y_{\mathrm{C}}}{Z_{\mathrm{C}}}+c_{v}} \end{array}\right], \quad \mathbf{X}_{\mathrm{C}}=\left[X_{\mathrm{C}} Y_{\mathrm{C}} Z_{\mathrm{C}}\right]^{T} \tag{1}$
其中 $[f_u \ f_v]^T$ 是焦距長度， $[c_u \ c_v]^T$ 是光心點。這里沒有考慮相機畸變，我們先提取特征點再求其去畸變后的坐標。

2.1.2 慣導信息

這部分參考系使用 $\mathrm B$ 表示，慣導信息流是時間間隔為 $\Delta t$ 的加速度測量值 $\mathbf {a_B}$ 和角速度測量值 $\boldsymbol{\omega_\mathrm B}$ 。這些測量值受噪聲和biases影響，包括加速度計偏置 $\mathbf b_a$ 和角速度偏置 $\mathbf b_g$ 。世界坐標系 $\mathrm W$ 下IMU的角度 $\mathbf{R}_{\mathrm{WB}} \in \mathrm{SO}(3)$ ，位置 $\mathrm{_Wp_B}$ 和速度 $\mathrm{_W}\mathbf v_\mathrm B$ ，需要注意的是加速度計需要去除重力 $\mathbf g_\mathrm w$ 的影響，計算公式如公式（2）所示^[3]：
$\begin{aligned} \mathbf{R}_{\mathrm{WB}}^{k+1} &=\mathbf{R}_{\mathrm{WB}}^{k} \operatorname{Exp}\left(\left(\boldsymbol{\omega}_{\mathrm{B}}^{k}-\boldsymbol_{g}^{k}\right) \Delta t\right) \\ _\mathrm{W} \mathbf{v}_{\mathrm{B}}^{k+1} &=_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{k}+\mathbf{g}_{\mathrm{W}} \Delta t+\mathbf{R}_{\mathrm{WB}}^{k}\left(\boldsymbol{a}_{\mathrm{B}}^{k}-\boldsymbol_{a}^{k}\right) \Delta t \\ _\mathrm{W} \mathbf{p}_{\mathrm{B}}^{k+1} &=_\mathrm{W} \mathbf{p}_{\mathrm{B}}^{k}+_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{k} \Delta t+\frac{1}{2} \mathbf{g}_{\mathrm{W}} \Delta t^{2}+\frac{1}{2} \mathbf{R}_{\mathrm{WB}}^{k}\left(\boldsymbol{a}_{\mathrm{B}}^{k}-\boldsymbol_{a}^{k}\right) \Delta t^{2} \end{aligned}\tag{2}$
連續(xù)關鍵幀之間的預積分項使用文章^[4]中算法來計算 $\Delta \mathbf R$ ， $\Delta \mathbf v$ 和 $\Delta \mathbf p$ ，公式表示為：
$\begin{aligned} \mathbf{R}_{\mathrm{WB}}^{i+1}=& \mathbf{R}_{\mathrm{WB}}^{i} \Delta \mathbf{R}_{i, i+1} \operatorname{Exp}\left(\left(\mathbf{J}_{\Delta R}^{g} \mathbf_{g}^{i}\right)\right) \\ _\mathrm{W} \mathbf{v}_{\mathrm{B}}^{i+1}=&_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i}+\mathbf{g}_{\mathrm{w}} \Delta t_{i, i+1} \\ &+\mathbf{R}_{\mathrm{WB}}^{i}\left(\Delta \mathbf{v}_{i, i+1}+\mathbf{J}_{\Delta_{v}}^{g} \mathbf_{g}^{i}+\mathbf{J}_{\Delta v}^{a} \mathbf_{a}^{i}\right) \\ _\mathrm{W} \mathbf{p}_{\mathrm{B}}^{i+1}=&_{\mathrm{W}} \mathbf{p}_{\mathrm{B}}^{i}+_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i} \Delta t_{i, i+1}+\frac{1}{2} \mathbf{g}_{\mathrm{w}} \Delta t_{i, i+1}^{2} \\ &+\mathbf{R}_{\mathrm{WB}}^{i}\left(\Delta \mathbf{p}_{i, i+1}+\mathbf{J}_{\Delta p}^{g} \mathbf_{g}^{i}+\mathbf{J}_{\Delta p}^{a} \mathbf_{a}^{i}\right) \end{aligned} \tag{3}$
這里雅可比 $\mathbf J^a_{(\cdot)}$ 和 $\mathbf J^g_{(\cdot)}$ 是當biases改變時對應項的一階近似，這樣不需要重新計算預積分。

相機和IMU參考系之間的變換關系為 $\mathbf{T}_{\mathrm{CB}}=\left[\mathbf{R}_{\mathrm{CB}} | _\mathrm{C}\mathbf P _\mathrm{B} \right]$ ，假設這個值通過標定可以事先知道。

2.2 視覺和慣導融合的ORB-SLAM

這部分詳細介紹了跟蹤、局部地圖和回環(huán)檢測三大線程的改變。

2.2.1 跟蹤

跟蹤部分負責以圖像幀率跟蹤傳感器位姿、速度和IMU的biases，這里替代原系統(tǒng)使用固定運動模型的方式，可以預測更加可靠的位姿。然后通過投影匹配地圖點，進行最小重投影誤差和IMU誤差的優(yōu)化。

<center>圖2. 跟蹤線程的優(yōu)化示意圖。a）跟蹤幀 $j$ （地圖改變），b）先驗（優(yōu)化結果），c）跟蹤幀 $j+1$ （地圖未改變），d）先驗（邊緣化），e）跟蹤 $j+2$ （地圖未改變），f）先驗（邊緣化）</center>

優(yōu)化過程根據(jù)地圖是否被局部建圖線程或閉環(huán)線程所更新分為兩種情況。

第一種如圖2中a）所示，當?shù)貓D改變或更新之后開始優(yōu)化幀 $j$ ，它和最新關鍵幀 $i$ 之間通過IMU約束關聯(lián)。優(yōu)化的狀態(tài)和公式如公式（4）所示。
$\begin{aligned} \theta&=\left\{ {\mathbf R}_{\mathrm {WB}}^j,_{\mathrm W}{\mathbf p}_\mathrm B^{j},_{\mathrm W}{\mathbf v}_\mathrm B^{j},{\mathbf b}_{g}^j,{\mathbf b}_a^j \right\} \\ \theta^*&=\arg\min\limits_{\theta}\left(\sum_k{\mathbf E}_{\mathrm {proj}}(k,j)+\mathbf E_{\mathrm {IMU}}(i,j )\right) \end{aligned} \tag 4$
這里特征點匹配對 $k$ 的重投影誤差 $\mathbf E_\mathrm {proj}$ 定義為：
$\begin{aligned} \mathbf{E}_{\mathrm {proj }}(k, j) &=\rho\left(\left(\mathbf{x}^{k}-\pi\left(\mathbf{X}_{\mathrm{C}}^{k}\right)\right)^{T} \mathbf{\Sigma}_{k}\left(\mathbf{x}^{k}-\pi\left(\mathbf{X}_{\mathrm{C}}^{k}\right)\right)\right) \\ \mathbf{X}_{\mathrm{C}}^{k} &=\mathbf{R}_{\mathrm{CB}} \mathbf{R}_{\mathrm{BW}}^{j}\left(\mathbf{X}_{\mathrm{W}}^{k}-_\mathbf{W} \mathbf{p}_{\mathrm{B}}^{j}\right)+_\mathrm{C}\mathbf{p}_{\mathrm{B}} \end{aligned} \tag {5}$
其中 $\mathrm x^k$ 為圖像上的特征點位置， $\mathbf X_\mathrm W^k$ 表示世界坐標系下的地圖點， $\boldsymbol {\Sigma_k}$ 表示和特征點尺度相關的信息矩陣。

IMU誤差項 $\mathbf E_{\mathrm {IMU}}$ 定義為：
$\begin{aligned} \mathbf{E}_{\mathrm{IMU}}(i, j)=& \rho\left(\left[\mathbf{e}_{R}^{T} \mathbf{e}_{v}^{T} \mathbf{e}_{p}^{T}\right] \boldsymbol{\Sigma}_{I}\left[\mathbf{e}_{R}^{T} \mathbf{e}_{v}^{T} \mathbf{e}_{p}^{T}\right]^{T}\right)+\rho\left(\mathbf{e}_^{T} \mathbf{\Sigma}_{R} \mathbf{e}_\right) \\ \mathbf{e}_{R}=& \log \left(\left(\Delta \mathbf{R}_{i j} \operatorname{Exp}\left(\mathbf{J}_{\Delta R}^{g} \mathbf_{g}^{j}\right)\right)^{T} \mathbf{R}_{\mathrm{BW}}^{i} \mathbf{R}_{\mathrm{WB}}^{j}\right) \\ \mathbf{e}_{v}=& \mathbf{R}_{\mathrm{BW}}^{i}\left(_\mathrm W \mathbf{v}_{\mathrm{B}}^{j}-_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i}-\mathbf{g}_{\mathrm{W}} \Delta t_{i j}\right) \\ &-\left(\Delta {\mathbf v}_{ij} + {\mathbf J}_{\Delta v}^ g{\mathbf b}_ g^j+{\mathbf J}_{\Delta v}^a{\mathbf b}_ a^j\right) \\ \mathbf{e}_{p}=& \mathbf{R}_{\mathrm{BW}}^{i}\left(_\mathrm W \mathbf{p}_{\mathrm{B}}^{j}-_\mathbf{W} \mathbf{p}_{\mathrm{B}}^{i}-_\mathrm W \mathbf{v}_{\mathrm{B}}^{i} {\Delta t}_{i j} -\frac{1}{2} \mathbf g _\mathrm W \Delta t_{ij}^2\right) \\ & -\left(\Delta {\mathbf p}_{ij}+{\mathbf J}_{\Delta p}^g{\mathbf b} _g^j+{\mathbf J}_{\Delta p}^a {\mathbf b} _a^j\right) \\ \mathbf{e}_=& \mathbf^{j}-\mathbf^{i} \end{aligned} \tag{6}$
這里 $\boldsymbol\Sigma_I$ 是預積分得到的信息矩陣， $\boldsymbol\Sigma_R$ 是bias的隨機游走^[3]， $\rho$ 是Huber魯棒核函數(shù)。

在使用高斯牛頓法算法進行優(yōu)化后，會對幀 $j$ 產(chǎn)生約束如圖2中b）所示，將得到的估計結果和海塞矩陣作為下一次優(yōu)化的先驗。

第二種如圖2中c）所示，此時地圖沒有改變，因此下一幀 $j+1$ 將會繼續(xù)加入優(yōu)化。但第 $j$ 幀無法保證被優(yōu)化的足夠準確，此時加入IMU約束會對 $j+1$ 產(chǎn)生錯誤的約束，因此需要將第 $j$ 幀一同加入到優(yōu)化中，上一次的優(yōu)化結果作為先驗加入優(yōu)化中。優(yōu)化的狀態(tài)和公式如公式（7）所示。
$\begin{aligned} \theta&=\left\{ {\mathbf R}_{\mathrm {WB}}^j,{\mathbf p}_{\mathrm W}^j, {\mathbf v}_{\mathrm W}^ j,{\mathbf b}_ g^j,{\mathbf b}_a^j,{\mathbf R}_{\mathrm {WB}}^{j+1},{\mathbf p}_{\mathrm W}^ {j+1},{\mathbf v}_{\mathrm W}^{j+1},{\mathbf b}_ g^{j+1},{\mathbf b}_a^{j+1}\right\} \\ \theta^*&=\arg\min\limits_{\theta}\left(\sum_k{\mathbf E}_{\mathrm {proj}}( k,j+1)+{\mathbf E}_{\mathrm {IMU}}( j, j+1) + {\mathbf E}_{\mathrm {prior}}(j)\right) \end{aligned} \tag{7}$
這里 ${\mathbf E}_{\mathrm {prior}}$ 為先驗項：
$\begin{aligned} {\mathbf E}_ {\mathrm {prior}}(j)&=\rho\left( [{\mathbf e}_R^T{\mathbf e}_v^T{\mathbf e}_p^T{\mathbf e}_b^T]\Sigma_ p[{\mathbf e}_R^T{\mathbf e}_v^T{\mathbf e}_p^T{\mathbf e}_b^T]^T\right) \\ {\mathbf e}_ R&={\mathrm {Log}}( \overline{\mathbf R}_{\mathrm {BW}}^j{\mathbf R}_{\mathrm {WB}}^ j) \quad \mathbf e_ v=_{\mathrm W}\overline {\mathbf v}_{\mathrm B}^j- _{\mathrm W} {\mathbf v}_{\mathrm B}^j \\ {\mathbf e}_ p&=_{\mathrm W}\overline{\mathbf p}_{\mathrm B}^j-_{\mathrm W}{\mathbf p}_{\mathrm B}^ j \quad {\mathbf e}_b=\overline{\mathbf b}^ j-{\mathbf b}^ j \end{aligned} \tag{8}$
這里 $(\overline \cdot)$ 和 $\boldsymbol \Sigma_p$ 是上一次優(yōu)化得到的狀態(tài)和海塞矩陣，即圖2中b）所示的。這次優(yōu)化完成后，邊緣化^[5]掉第 $j$ 幀得到先驗，即圖2中d）。這樣當下一幀 $j+2$ 到來，地圖未發(fā)生更新，就重復兩幀的優(yōu)化以及邊緣化過程，如圖2中e）和f）所示。一旦地圖被更新之后，會導致之前的先驗信息無法提供正確的約束，因此會重新變成與上一關鍵幀的約束即圖2中a）所示。

2.2.2 局部建圖

這一部分在插入新的關鍵幀后執(zhí)行局部BA（Bundle Adjustment）優(yōu)化。優(yōu)化最新的 $N$ 個關鍵幀構成的局部窗口以及它們觀測到的所有點。和局部窗口中的關鍵幀具有共視關系（共視圖中相連）卻不在局部窗口的關鍵幀構成固定窗口，貢獻總的殘差，但狀態(tài)固定不被優(yōu)化。第 $N+1$ 個最新的關鍵幀總是包含在固定窗口中來提供IMU約束。圖3表明了原始的ORB-SLAM和VI-ORB-SLAM在局部BA中的線程中的區(qū)別。誤差函數(shù)結合了重投影誤差和IMU誤差項如公式（5）和（6）所示。

<center>圖3. 原版ORB-SLAM（上方）和本文VI-ORB-SLAM（下方）的局部BA對比。VI-ORB-SLAM的局部窗口提取的是按照時間順序的關鍵幀，而ORB-SLAM中是通過共視圖提取的。</center>

需要注意的是IMU的引入，導致每個關鍵幀增加了9個優(yōu)化的變量，因此也變得更加復雜，需要選擇合適的窗口大小來保證實時性。

局部地圖同時也負責關鍵幀的管理。原版ORB-SLAM會有丟棄不好關鍵幀的策略，保證地圖的大小合適，但在VI-ORB-SLAM中將會帶來問題。因為IMU約束的積分時間越長，誤差越大。因此我們需要設置以下策略來約束關鍵幀的管理：

局部BA窗口中的兩個連續(xù)關鍵幀之間的時間不能大于0.5s
閉環(huán)后執(zhí)行的full-BA中連續(xù)兩個關鍵幀之間時間不能大于3s

2.2.3 環(huán)路閉合

閉環(huán)線程的目的是當回到建過地圖的位置時，減少運行中的累計漂移。方法為利用地點重識別模塊檢測和之前的關鍵幀進行匹配，然后計算二者間剛體變換^[6]，最后進行一個位姿圖優(yōu)化。

由于尺度可觀，這里的剛體變換不是使用原來的7DoF，而是6DoF的。位姿圖優(yōu)化不對速度和IMU的biases進行優(yōu)化。這顯然不是最優(yōu)的，而bias和速度是局部準確的，可以在之后繼續(xù)使用IMU的信息。然后會在并行線程進行full-BA，來優(yōu)化包括bias和速度的所有狀態(tài)。

2.3 IMU初始化

這部分主要在已知一系列ORB-SLAM關鍵幀后，計算視覺和慣導相融合的full-BA所需要估計的尺度、重力方向、速度和IMU biases的初始估計。思想就是讓ORB-SLAM運行一段時間使得各個狀態(tài)都變得可觀。這種初始化方法適用于任何其它的SLAM系統(tǒng)，只需要保證兩個連續(xù)的關鍵幀時間上接近即可。

初始化使用分而治之思想，分為以下子問題：

(1) IMU陀螺儀的bias估計

(2) 不考慮加速度計的bias，尺度和重力近似估計

(3) 加速度計bias估計，尺度和重力方向求精

(4) 速度估計

2.3.1 陀螺儀bias估計

陀螺儀bias可以從已知的兩個連續(xù)關鍵幀間旋轉估計出來。假設陀螺儀的bias變化很小，視為恒定。通過最小化陀螺儀積分和ORB-SLAM求出的相對角度差異求出。對于連續(xù)關鍵幀有：
$\underset{\mathrm_{s}}{\operatorname{argmin}} \sum_{i=1}^{N-1}\left\|\operatorname{Log} \left(\left(\Delta \mathbf{R}_{i, i+1} \operatorname{Exp}\left(\mathbf{J}_{\Delta R}^{g} \mathbf_{g}\right)\right)^{T} \mathbf{R}_{\mathrm{BW}}^{i+1} \mathbf{R}_{\mathrm{WB}}^{i}\right)\right\|^{2} \tag{9}$
這里 $N$ 是關鍵幀數(shù)目， $\mathbf{R}_\mathrm{WB}^{(\cdot)} = \mathbf{R}_\mathrm{WC}^{(\cdot)}\mathbf{R}_\mathrm{CB}$ 是由ORB-SLAM計算出來的 $\mathbf{R}_\mathrm{WC}^{(\cdot)}$ 和標定的 $\mathbf{R}_\mathrm{CB}$ 計算得到， $\Delta \mathbf{R}_{i, i+1}$ 是兩個連續(xù)關鍵幀之間的陀螺儀積分得到。

2.3.2 尺度和重力估計（未考慮加速度計bias）

ORB-SLAM計算得到的尺度是任意的，因此從相機系變換到IMU坐標系時需要考慮一個尺度因子 $s$ :
$_{\mathrm W}{\mathbf p}_{\mathrm B}= s_{\mathrm W} {\mathbf p}_{\mathrm C} +{\mathbf R}_{\mathrm {WC}} \ _{\mathrm C}{\mathbf p}_{\mathrm B} \tag{10}$
將公式（10）帶入公式（3）中兩個連續(xù)關鍵幀之間的相對位置方程，忽略加速度計的bias，得到：
$s\ _{\mathrm W}{\mathbf p}_{\mathrm C}^ {i+1}= s \ _{\mathrm W}{\mathbf p}_{\mathrm C}^i + _{\mathrm W}{\mathbf v}_{\mathrm B}^i \Delta t_{i,i+1}+\frac12 \mathbf g_W\Delta t_{i,i+1}^2+{\mathbf R}_{\mathrm {WB}}^i \Delta {\mathbf p}_{ i,i+1}+({\mathbf R}_{\mathrm {WC}}^i-{\mathbf R}_{\mathrm {WC}}^{i+1}) _{\mathrm C}{\mathbf p}_\mathrm B \tag{11}$
通過這個線性方程求解的目的是得到尺度 $s$ 和 $\mathbf g_\mathrm W$ ，為了減小復雜度避免求解 $N$ 個關鍵幀的速度，考慮連續(xù)三個關鍵幀如公式（11）兩兩關系和公式（3）中速度關系得到：
$\begin{bmatrix}\boldsymbol\lambda( i) \ \boldsymbol\beta( i)\end{bmatrix} {\begin{bmatrix} s\\\mathbf g_\mathrm w\end{bmatrix}}=\boldsymbol\gamma(i) \tag {12}$
為了符號清晰，這里我們把 $i,i+1,i+2$ 三個關鍵幀使用1，2，3代表，有：
$\begin{aligned} \boldsymbol\lambda(i)=&\left(_\mathrm{W} \mathbf p_{\mathrm{C}}^{2}-_\mathrm{W} \mathbf{p}_{\mathrm{C}}^{1}\right) \Delta t_{23}-\left(_\mathrm{W} \mathbf{P}_{\mathrm{C}}^{3}-_\mathrm{W} \mathbf{P}_{\mathrm{C}}^{2}\right) \Delta t_{12} \\ \boldsymbol{\beta}(i)=& \frac{1}{2} \mathbf{I}_{3 \times 3}\left(\Delta t_{12}^{2} \Delta t_{23}+\Delta t_{23}^{2} \Delta t_{12}\right) \\ \boldsymbol\gamma(i)=&\left(\mathbf{R}_{\mathrm{WC}}^{2}-\mathbf{R}_{\mathrm{WC}}^{1}\right) {_{\mathrm{C}}} \mathbf p _ \mathrm B \Delta t_{23}-\left(\mathbf{R}_{\mathrm{WC}}^{3}-\mathbf{R}_{\mathrm{WC}}^{2}\right){_{\mathrm{C}}} \mathbf p _ \mathrm B \Delta t_{12} \\ &+\mathbf{R}_{\mathrm{WB}}^{2} \Delta \mathbf{p}_{23} \Delta t_{12}+\mathbf{R}_{\mathrm{WB}}^{1} \Delta \mathbf{v}_{12} \Delta t_{12} \Delta t_{23} \\ &-\mathbf{R}_{\mathrm{WB}}^{1} \Delta \mathbf{p}_{12} \Delta t_{23} \end{aligned} \tag{13}$
我們將所有的三個連續(xù)關鍵幀得到的關系寫成 $\mathbf A_{3(N-2)\times4}\mathbf x_{4\times1} = \mathbf B _{3(N-2)\times1}$ 形式，通過SVD分解求得尺度因子 $s^*$ 和重力向量 $\mathbf g_\mathrm W^*$ 。因為我們有 $3(N-2)$ 個方程和4個未知數(shù)，因此需要至少4個關鍵幀。

2.3.3 加速度計bias估計，尺度和重力方向求精

到目前為止還未考慮加速度計的bias影響，加速度計bias很容易淹沒在重力加速度中^[7]，因而在公式（12）中增加加速度計bias增加了系統(tǒng)病態(tài)的幾率。為增加可觀性，引入重力模長為 $G$ 的約束。使用重力方向為 $\hat{\mathbf g}_\mathrm I = \left\{0,0,-1 \right\}$ 慣性參考系 $I$ 和計算出來的重力方向 $\hat {\mathbf g }_\mathrm W = {\mathbf g }_\mathrm W^* / \|{\mathbf g }_\mathrm W^*\|$ ，可以計算出來 $\mathbf R_\mathrm {WI}$ 如下：
$\begin{aligned} {\mathbf R}_{\mathrm {WI}}&=\operatorname{Exp}(\hat {\mathbf v}\theta) \\ \hat{\mathbf v}&=\frac{\hat{\mathbf g}_\mathrm I\times\hat{\mathbf g}_\mathrm W}{||\hat{\mathbf g}_\mathrm I\times\hat{\mathbf g}_\mathrm W||} , \quad \theta=\operatorname{atan2}(||\hat{\mathbf g}_\mathrm I\times\hat{\mathbf g}_\mathrm W||,\hat{\mathbf g}_\mathrm I\cdot \hat{\mathbf g}_\mathrm W) \end{aligned}\tag {14}$
進而重力向量為
$\mathbf g_\mathrm W = \mathbf R _ \mathrm{WI} \hat{\mathbf g}_{\mathrm I}G \tag{15}$
這里 $\mathbf R_\mathrm {WI}$ 可以使用兩個繞x軸和y軸的角度，因為繞z軸旋轉是不影響重力向量的。使用SO3上的擾動：
$\begin{aligned} \mathbf g_\mathrm w&={\mathbf R}_{\mathrm {WI}}\operatorname{Exp}(\delta\theta)\hat {\mathbf g}_\mathrm I G \\ \boldsymbol {\delta\theta}&=\begin{bmatrix} \boldsymbol{\delta \theta_{xy}^T} &0\end{bmatrix}^T, \quad \boldsymbol{ \delta \theta_{xy}} = \begin{bmatrix}\delta \theta_x &\delta \theta _y\end{bmatrix}^T \end{aligned} \tag {16}$
對其進行一階近似：
$\mathbf g_ \mathrm w \approx {\mathbf R}_{\mathrm {WI}} \hat {\mathbf g}_\mathrm I G -{\mathbf R}_{\mathrm {WI}} (\hat {\mathbf g}_\mathrm I)_\times G \boldsymbol{\bf \delta\theta} \tag{17}$
將公式（17）帶入公式（11）并考慮加速度計的bias，得到：
$\begin{aligned} s_{\mathrm{W}} \mathbf{p}_{\mathrm{C}}^{i+1}=& s_{\mathrm{W}} \mathbf{p}_{\mathrm{C}}^{i}+_{\mathrm{W}} \mathbf{v}_{\mathrm{B}}^{i} \Delta t_{i, i+1}-\frac{1}{2} \mathbf{R}_{\mathrm{WI}}\left(\hat{\mathbf{g}}_{\mathrm{I}}\right)_{\times} G \Delta t_{i, i+1}^{2} \boldsymbol{\delta} \boldsymbol{\theta} \\ &+\mathbf{R}_{\mathrm{WB}}^{i}\left(\Delta \mathbf{p}_{i, i+1}+\mathbf{J}_{\Delta p}^{a} \mathbf_{a}\right)+\left(\mathbf{R}_{\mathrm{WC}}^{i}-\mathbf{R}_{\mathrm{WC}}^{i+1}\right) {_\mathrm{C} \mathbf{P}_{\mathrm{B}}} \\ &+\frac{1}{2} \mathbf{R}_{\mathrm{WI}} \hat{\mathbf{g}}_{\mathrm{I}} G \Delta t_{i, i+1}^{2} \end{aligned} \tag{18}$
使用公式（12）的三個連續(xù)關鍵幀關系可以消掉速度得到：
$\begin{bmatrix}\boldsymbol \lambda(i)\ &\boldsymbol \phi(i)\ &\boldsymbol \zeta(i)\end{bmatrix}\begin{bmatrix} s\\ \boldsymbol {\delta\theta_{xy}}\\\mathbf b_a\end{bmatrix}=\boldsymbol \psi(i) \tag{19}$
這里 $\boldsymbol \lambda(i)$ 和公式（13）保持相同， $\boldsymbol \phi(i)$ ， $\boldsymbol \zeta(i)$ 和 $\boldsymbol \psi(i)$ 計算如下：
$\begin{aligned} \boldsymbol\phi(i)=&\left[\frac 12{\mathbf R}_{\mathrm {WI}}(\hat {\mathbf g}_\mathrm I) G(\Delta t_{12}^2\Delta t_{23}+\Delta t_{23}^2\Delta t_{12})\right]_{(:,1:2)} \\ \boldsymbol\zeta(i)=&{\mathbf R}_{\mathrm {WB}}^2{\mathbf J}_{\Delta p23}^a\Delta t_{12}+{\mathbf R}_\mathrm {WB}^1{\mathbf J}_{\Delta v_{23}}^a\Delta t_{12}\Delta t_{23}\\ &-{\mathbf R}_{\mathrm {WB}}^1{\mathbf J}_{\Delta p_{12}}^a\Delta t_{23} \\ \boldsymbol\psi(i)=&({\mathbf R}_{\mathrm {WC}}^2-{\mathbf R}_{\mathrm {WC}}^1) _{\mathrm C}{\mathbf p}_{\mathrm B}\Delta t_{23}-({\mathbf R}_{\mathrm {WC}}^3-{\mathbf R}_{\mathrm {WC}}^2){_{\mathrm C}{\bf p}}_{\mathrm B}\Delta t_{12}\\ &+{\mathbf R}_{\mathrm {WB}}^2\Delta {\mathbf p}_{23}\Delta t_{12}+{\mathbf R}_{\mathrm {WB}}^1\Delta {\mathbf v}_{12}\Delta t_{12}\Delta t_{23}\\ &-{\mathbf R}_{\mathrm {WB}}^1\Delta {\mathbf p}_{12}\Delta t_{23}+\frac 12{\mathbf R}_{\mathrm {WI}}\hat {\mathbf g}_{\mathrm I} G\Delta t_{ij}^2 \end{aligned} \tag{20}$
這里 $[]_{(:,1:2)}$ 表示矩陣前兩列。將所有三個連續(xù)關鍵幀之間如公式（19）的關系列成 $\mathbf A_{3(N-2)\times6}\mathbf x_{6\times1} = \mathbf B _{3(N-2)\times1}$ 形式，使用SVD分解可以求解出，尺度因子 $s^*$ ，重力方向糾正角 $\boldsymbol {\delta\theta_{xy}^*}$ 和加速度bias $\mathbf b_a^*$ 。因為我們有 $3(N-2)$ 個方程和6個未知數(shù)，因此需要至少4個關鍵幀。我們可以通過計算條件數(shù)（最大最小奇異值之間的比）來判斷系統(tǒng)是否狀態(tài)良好（即運動是否足夠使得變量可觀）。

2.3.4 速度估計

目前為止我們還沒得到速度的 $3N$ 個未知數(shù)。由于其它變量已知，我們可以使用公式（18）求出所有關鍵幀的速度，對于最新的幾個關鍵幀可以使用公式（3）中的關系求出速度。

2.3.5 重定位后的bias重新初始化

當發(fā)生重定位后，使用公式（9）求出陀螺儀bias，此時已知尺度和重力公式（19）得到了簡化，直接求出加速度計bias。本系統(tǒng)使用只用視覺估計的20個連續(xù)幀計算這兩個bias。

3. 實驗分析

這部分對本文初始化算法和VI-ORB-SLAM算法在EuRoC數(shù)據(jù)集^[8]上的左側單目進行了評估。

A. IMU初始化

在V2_01_easy序列對初始化進行測試，提取每個關鍵幀插入時系統(tǒng)的信息，并保證一直處于初始化狀態(tài)，來分析各個狀態(tài)的收斂情況。

<center> 圖4. 在V2_01_easy進行IMU初始化 </center>

從圖4中可以發(fā)現(xiàn)，在10-15秒之間所有的變量都收斂到穩(wěn)定值且尺度因子也接近最優(yōu)值。圖4也展示了公式（19）的條件數(shù)，表示需要時間來使系統(tǒng)達到穩(wěn)定的狀態(tài)。這確保了傳感器充分運動使得所有的狀態(tài)都變得可觀，尤其是對于加速度計bias。圖4中最后一行顯示了所花費的時間，是呈線性增長的，但這里不包括公式（12）和（19）的求解。從實驗中可以看出，將初始化拆分為簡單的子問題，得到了更好的效率和效果。

本文提出的初始化方法保證了融合IMU時具有一個比較可靠的重力，biases，尺度和速度估計值。對于RuRoC數(shù)據(jù)集，我們發(fā)現(xiàn)15s可以得到一個較精確的初始化結果。未來的工作需要去制定一個自動策略來判斷是否初始化完成。我們在實驗中發(fā)現(xiàn)對于條件數(shù)制定一個絕對閾值的策略是不足夠可靠的。

B. SLAM評估并與SOTA的對比

在EuRoC數(shù)據(jù)集上測試了VI-ORB-SLAM的精度。局部BA的局部窗口大小設置為10個關鍵幀，IMU初始化在運行15秒后執(zhí)行。表格一展示了每一個序列關鍵幀軌跡的位移RMSE（Root Mean Square Error），同時也測得了軌跡和重建的尺度誤差。VI-ORB-SLAM除了V1_03_difficult其它的序列都可以成功實時運行，因為改序列運動太極端以致于無法完成15s的初始化。最終系統(tǒng)實現(xiàn)了小于1%的尺度恢復誤差，在20平方米的房間環(huán)境內(nèi)達到3cm的典型精度，在300平方米的工廠環(huán)境中達到8cm的典型精度。為了展示尺度誤差對精度的損失，表一中還給出了尺度對齊后的精度，詳見GT scale列。同時表一也說明了進行視覺慣性的full-BA有助于尺度和姿態(tài)的估計，詳見 Full BA列。對于序列V1_02_medium序列的重建結果如圖1所示。

<center> 表格一 EuRoC數(shù)據(jù)集上關鍵幀軌跡精度</center>

在表一中，使用ORB-SLAM作為對比基準，VI-ORB-SLAM能夠處理V2_03_difficult更加魯棒，并且能夠恢復度量尺度防止尺度漂移。它的精度接近具有準確尺度的ORB-SLAM系統(tǒng)，但由于IMU的加入BA優(yōu)化的代價變得更加昂貴，所以局部BA的窗口大小會小于ORB-SLAM。這解釋了沒有進行full-BA的VI-ORB-SLAM效果差于ORB-SLAM的原因。實際上VI-ORB-SLAM的full-BA會在7秒內(nèi)的15次迭代后收斂，而ORB-SLAM在小于1秒的時間內(nèi)5次迭代即可收斂。

為了測試VI-ORB-SLAM的重用地圖能力，首先在第一個序列運行并執(zhí)行full-BA得到環(huán)境地圖。然后再運行剩余的序列進行重定位并運行SLAM。由于存在地圖V1_03_difficult也可以成功定位。在V1，V2和MH的環(huán)境中RMSE分別為0.037，0.027和0.076，尺度因子誤差為1.2%，0.1%和0.2%。結果表明了當重新回到原場景后可以得到無漂移累積的定位，因為RMSE沒有變得更大。

<center>圖5 本文方法和出色的直接雙目VIO[9]的相對位姿誤差（RPE）對比。由于地圖重用，本文SLAM誤差沒有隨著軌跡增長增加，且[9]中使用雙目，本文使用單目。

最后將VI-ORB-SLAM和目前出色的直接法雙目VIO（Visual-Inertial Odometry）^[9]]進行對比。由于地圖重用機制，本文算法沒有誤差累積，而VIO算法隨著軌跡長度誤差增加。與雙目直接VIO方法對比，本文基于特征點的方法效果顯著。

4. 總結

本文提出了一種新穎的緊耦合視覺和慣導融合的SLAM系統(tǒng)，并且具有實時閉環(huán)檢測和地圖重用能力。這使得實現(xiàn)了零漂移定位，而VIO算法漂移會無限制增長。實驗表明了本文單目SLAM算法恢復尺度同時具有高精度，相同環(huán)境下取得了比目前杰出系統(tǒng)更加優(yōu)秀的效果。這種零漂移定位是虛擬/增強現(xiàn)實（VR/AR，Virtual/Augmented Reality），這種在相同區(qū)域操作的系統(tǒng)所必須的特性。更多地，如果使用雙目或者RGB-D相機能夠得到更好的精度和魯棒性，并且可以大大簡化初始化過程。本文初始化算法的不足之處在于依賴單目SLAM的初始化效果，希望接下來研究可以利用陀螺儀使得單目初始化更加迅速且魯棒。

參考文獻

R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Aug. 2015. ?
R. Mur-Artal and J. D. Tardos, “ORB-SLAM2: an Open-Source SLAM system for monocular, stereo and RGB-D cameras,” arXiv:1610.06475, Oct. 2016. ?
C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., to be published. doi: 10.1109/TRO.2016.2597321. ? ?
T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for highdynamic motion in built environments without initial conditions,” IEEE Trans. Robot., vol. 28, no. 1, pp. 61–76, Feb. 2012 ?
S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual–inertial odometry using nonlinear optimization,” Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, Aug. 2015. ?
B. K. P. Horn, “Closed-form solution of absolute orientation using unit quaternions,” J. Opt. Soc. Amer. A, vol. 4, no. 4, pp. 629–642, 987. ?
A. Martinelli, “Closed-form solution of visual-inertial structure from motion,” Int. J. Comput. Vis., vol. 106, no. 2, pp. 138–152, Jan. 2014. ?
M. Burri et al., “The EuRoC micro aerial vehicle datasets,” Int. J. Robot. Res., vol. 35, no. 10, pp. 1157–1163, Jan. 2016. ?
V. Usenko, J. Engel, J. Stueckler, and D. Cremers, “Direct visual-inertial odometry with stereo cameras,” in Proc. IEEE Int. Conf. Robot. Autom., 2016, pp. 1885–1892. ?

色偷偷精品伊人,欧洲久久精品,欧美综合婷婷骚逼,国产AV主播,国产最新探花在线,九色在线视频一区,伊人大交九欧美,1769亚洲,黄色成人av

Visual-Inertial Monocular SLAM With Map Reuse總結