Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization

Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization¹

• 把尺度，重力方向放進模型中進行聯合優化
• 由於尺度不是立刻就可觀的，因此我們以任一尺度進行初始化，而不是延遲初始化到量都變得可觀之後（VIORB）。
• 對舊的量進行部分邊緣化，爲了保證一致性，提出“動態邊緣化”策略。因此在尺度遠離最優值時也可以採用邊緣化。

DIRECT SPARSE VISUAL-INERTIAL ODOMETRY

DSVIO

初始化和可觀性

零速和恆速運動都會導致無法進行初始化，VIORB使用15s初始化來保證所有量都可觀。

我們是把scale，gravity都放入優化中使其和pose一起優化。

• 使用DSO的初始化方式，得到粗略位姿估計，和近似的逆深度，保證平均逆深度爲1.
• 初始重力方向採用40個加速度測量數據的平均值。
• 初始化速度和IMU-bias的值爲0.

基於Sim(3)的世界表示方式

爲了能夠以尺度，重力初始值來優化。除了度量座標系，定義一個DSO座標系，它是度量系的一個被尺度放縮，和旋轉的版本。定義爲$\mathbf{T}_{m\_d} \in\{\mathbf{T} \in \operatorname{SIM}(3) | \text { translation }(\mathbf{T})=0\}$，$\xi_{m\_d} \in \log(\mathbf T_{m\_d}) \in \mathfrak sim(3)$.

光度誤差使用DSO系，獨立於尺度和重力方向，慣導誤差使用度量系（metric frame）

Scale-aware Visual-inertial Optimization

關鍵幀之間不能超過0.5s，邊緣化中可以違反這個部分，確保長期關聯。優化的位姿是在DSO座標系下的，不依賴於環境尺度。

1) Nonlinear Optimization

每個關鍵幀優化的變量爲：
$\boldsymbol{s}_{i}:=\left[\left(\boldsymbol{\xi}_{c a m_{i-w}}^{D}\right)^{T}, \boldsymbol{v}_{i}^{T}, \boldsymbol{b}_{i}^{T}, a_{i}, b_{i}, d_{i}^{1}, d_{i}^{2}, \ldots, d_{i}^{m}\right]^{T} \tag{1}$
所有的向量爲：
$\boldsymbol{s}=\left[\boldsymbol{c}^{T}, \boldsymbol{\xi}_{m_{-} d}^{T}, \boldsymbol{s}_{1}^{T}, \boldsymbol{s}_{2}^{T}, \ldots, \boldsymbol{s}_{n}^{T}\right]^{T} \tag{2}$
相機和IMU的的residual是沒有重疊部分的，因此可以獨立開：
$\mathbf{H}=\mathbf{H}_{\mathrm{photo}}+\mathbf{H}_{\mathrm{imu}} \text { and } \boldsymbol{b}=\boldsymbol{b}_{\mathrm{photo}}+\boldsymbol{b}_{\mathrm{imu}} \tag{3}$
慣性器件的殘差需要使用相對IMU的metric座標系，因此定義新的狀態：
$\boldsymbol{s}_{i}^{\prime}:=\left[\boldsymbol{\xi}_{w_{-}i m u_{i}}^{M}, \boldsymbol{v}_{i}, \boldsymbol{b}_{i}\right]^{T} \text { and } \boldsymbol{s}^{\prime}=\left[\boldsymbol{s}_{1}^{\prime T}, \boldsymbol{s}_{2}^{\prime T}, \ldots, \boldsymbol{s}_{n}^{\prime T}\right]^{T} \tag{4}$
慣性的殘差導出：
$\mathbf{H}_{\mathrm{imu}}^{\prime}=\mathbf{J}_{\mathrm{imu}}^{\prime T} \mathbf{W}_{\mathrm{imu}} \mathbf{J}_{\mathrm{imu}}^{\prime} \text { and } \boldsymbol{b}_{\mathrm{imu}}^{\prime}=-\mathbf{J}_{\mathrm{imu}}^{\prime T} \mathbf{W}_{\mathrm{imu}} \boldsymbol{r}_{\mathrm{imu}} \tag{5}$
這個之前的位姿表示定義不同，需要一個$\mathbf {J_{rel} }$
$\mathbf{H}_{\mathrm{imu}}=\mathbf{J}_{\mathrm{rel}}^{T} \cdot \mathbf{H}_{\mathrm{imu}}^{\prime} \cdot \mathbf{J}_{\mathrm{rel}} \text { and } b_{\mathrm{imu}}=\mathbf{J}_{\mathrm{rel}}^{T} \cdot \boldsymbol{b}_{\mathrm{imu}}^{\prime} \tag{6}$
詳細推導見補充材料。

2）Marginalization using the Schur-Complement:

這裏爲了保證零空間，會使用FEJ，把$\mathbf {J_{rel} }$固定線性化點，他是和$\xi_{m\_d}$相關的。

3）Dynamic Marginalization for Delayed Scale Convergence

邊緣化時不固定尺度的線性化點。

動態邊緣化就是保持幾個邊緣化先驗，當尺度估計離線性化點交遠時，重置當前使用的。保留三個邊緣化先驗，$M_{visual},M_{curr},M_{half}$

$M_{visual}$包含之前視覺狀態的與尺度無關的信息，不能用來推斷全局尺度。

$M_{curr}$包含設置線性點以來的所有尺度信息。

$M_{half}$包含最近尺度接近當前值的狀態信息。

當尺度估計和線性點差很多時，設置$M_{curr}=M_{half}，M_{curr}=M_{visual}$，確保優化有之前的信息。

定義$G_{metric}$包含VI factor，$G_{visual}$包含其它的factor，不包括邊緣化先驗

$G_{\text {full }}=G_{\text {metric }} \cup G_{\text {visual }} \tag{7}$
優化使用的因子圖集是：
$G_{b a}=G_{\text {metric }} \cup G_{\text {visual }} \cup M_{\text {curr }} \tag{8}$

當一個關鍵幀被邊緣化掉，我們在因子圖$G_{\text {visual }}\cup M_{\text {visual }}$上進行邊緣化操作來更新$M_{\text {visual }}$，意味着它包含所有的邊緣化的視覺因子，沒有慣性因子，因此和尺度無關。

對於邊緣化的關鍵幀，定義$i$幀被邊緣化時的尺度估計值爲$s_i$，包含慣性因子被邊緣化時，強制下面的約束：
$\forall i \in M_{\mathrm{curr}}: s_{i} \in\left[s_{\mathrm{middle}} / d_{i}, s_{\text {middle }} \cdot d_{i}\right] \tag{9}$

$\forall i \in M_{\text {half }}: s_{i} \in\left\{\begin{array}{ll}{\left[s_{\text {middle }}, s_{\text {middle }} \cdot d_{i}\right],} & {\text { if } s_{\text {curr }}>s_{\text {middle }}} \\ {\left[s_{\text {middle }} / d_{i}, s_{\text {middle }}\right],} & {\text { otherwise }}\end{array}\right. \tag{10}$

$s_{middle}$是當前允許尺度範圍的中間值，$d_i$是時間$i$尺度區間大小，$s_{curr}$是當前的尺度估計

我們在因子圖$G_{ba}$上面邊緣化更新$M_{curr}$，在因子圖
$G_{\text {metric }} \cup G_{\text {visual }} \cup M_{\text {half }}$上邊緣化更新 $M_{half}$.

爲了保證尺度約束，邊緣化後使用算法：

作用：

• 確保當前使用的邊緣化因子中尺度的差異小於$d_i^2$的約束條件被滿足
• 另一方面，因子中總是包含慣性因子，因此可以持續進行尺度估計
• FEJ被分別使用在$M_{curr}$和$M_{half}$中

這裏的$d_i$選擇應該足夠小來保證一致性，也不能太小需要確保包含慣性因子，因此使用動態調整：
$d_{i}=\min \left\{d_{\min }^{j} | j \in \mathbb{N} \backslash\{0\}, \frac{s_{i}}{s_{i-1}}<d_{i}\right\} \tag{11}$
理解：$d_i$太小則容易按照算法1進行更新，視覺把curr給佔據了。$d_i$太大了則覆蓋的範圍小，則無法保證和之前的狀態有一致性。

公式（11）確保不會發生，$M_{half}$被$M_{visual}$給重置，然後$M_{half}$又賦值給了$M_{curr}$，我們選擇$d_{\min }=\sqrt{1.1}$

每個新幀聯合優化完成後，跟蹤會使用新估計的尺度、重力方向、bias和速度重新初始化，以及設置新的關鍵幀爲參考幀。當估計完新的一幀變量，邊緣化掉所有變量除了關鍵幀pose和最新幀的變量。因爲沒有涉及尺度，不需要動態邊緣化。

Supplementary Material

對參數的評價

• 由於IMU的加入，點可以少一些，而且點少還會提高精度，因爲可靠地點數目變多了

公式推導

使用以下公式來判斷尺度是否收斂，n是隊列裏最大數目取60：
$c=\frac{\max _{j:=i-n+1}^{n} s_{j}}{\min _{j:=i-n+1}^{n} s_{j}}-1 \tag{12}$
當$c<0.005$時認爲收斂，固定$\xi_{m\_d}$。

計算$\mathbf{{J}_{rel}}$

相機和IMU是在兩個不同的座標系下表示的，因此位姿變換有
$\begin{aligned} \mathbf{T}^{M}_{w\_imu} &= \mathbf{T}_{m\_d}(\mathbf{T}^D_{cam\_w})^{-1}(\mathbf{T}_{m\_d})^{-1}\mathbf{T}^M_{cam\_imu} \\\xi^M_{w_{-}imu} &=\boldsymbol{\xi}_{m_{-} d} \boxplus \left(\boldsymbol{\xi}_{c a m_{-} w}^{D}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \xi_{c a m_{-} imu}^{M} \\&=: \Psi\left(\boldsymbol{\xi}_{cam_{-} w}^{D}, \boldsymbol{\xi}_{m\_{d}}\right) \end{aligned}\tag{13}$
公式含義是：Metric系下imu轉到cam變換矩陣，先轉到DSO系下，DSO系下轉爲imu到world變換矩陣，再把DSO系轉爲Metric系得到結果

對於函數$\Omega(\xi)：\mathfrak {sim}(3) \to \mathfrak{sim}(3)$，我們定義導數：
$\Omega(\boldsymbol{\xi})^{-1} \boxplus \Omega(\boldsymbol{\xi} \boxplus\boldsymbol{\epsilon})=\frac{d \Omega(\boldsymbol{\xi} \boxplus\boldsymbol{\epsilon})}{d \boldsymbol{\epsilon}} \cdot \boldsymbol{\epsilon}+\eta(\boldsymbol{\epsilon}) \cdot \boldsymbol{\epsilon} \tag{14}$
$\eta(\epsilon) \to 0$當$\| \epsilon \| \to 0$

對於函數$f(\boldsymbol{\xi}): \mathfrak{sim}(3) \rightarrow \mathbb{R}$，導數爲
$\frac{d f(\Omega(\boldsymbol{\xi} \boxplus \boldsymbol \epsilon ))}{d \boldsymbol{\epsilon}}=\frac{d f(\Omega(\boldsymbol{\xi}) \boxplus\boldsymbol \delta)}{\delta} \cdot \frac{d \Omega(\boldsymbol{\xi} \boxplus \boldsymbol{\epsilon})}{\epsilon} \tag{15}$
1） 下面推導Jacobian相對於位姿導數
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \boldsymbol{\epsilon}, \boldsymbol{\xi}_{m\_d}\right)}{\partial \boldsymbol{\epsilon}}$
其中伴隨性質有：
$\mathbf{T} \cdot \exp (\boldsymbol{\epsilon})=\exp \left(\mathrm{Adj}_{\mathbf{T}} \cdot \boldsymbol{\epsilon}\right) \cdot \mathbf{T}\\ \log \left(\mathbf{T} \cdot \exp (\boldsymbol{\epsilon}) \cdot \mathbf{T}^{-1}\right)=\operatorname{Adj}_{\mathbf{T}} \cdot \boldsymbol{\epsilon} \tag{16}$
其中$\mathbf{T} \in \mathbf {SIM}(3)$，使用公式（13）（16）的結果
$\begin{aligned} &\Psi\left(\boldsymbol{\xi}_{c a m_{-w}}^{D}, \boldsymbol{\xi}_{m_{-d}}\right)^{-1} \boxplus \Psi\left(\boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \boldsymbol{\epsilon}, \boldsymbol{\xi}_{m_{-} d}\right) \\ &=\left(\boldsymbol{\xi}_{m_{-} d} \boxplus\left(\boldsymbol{\xi}_{c a m_-w}^{D}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \boldsymbol{\xi}_{c a m_{-}i m u}^{M}\right)^{-1} \boxplus\left(\boldsymbol{\xi}_{m_{-} d} \boxplus\left(\boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \boldsymbol{\epsilon}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \boldsymbol{\xi}_{c a m_{-}i m u}^{M}\right) \\ &=\left(\boldsymbol{\xi}_{c a m_{-}i m u}^{M}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d} \boxplus \boldsymbol{\xi}_{c a m_{-} w}^{D} \boxplus \underbrace{\boldsymbol{\xi}_{m_{-}d}^{-1} \boxplus \boldsymbol{\xi}_{m_{-}d}}_{=0} \boxplus \boldsymbol{\epsilon}^{-1} \boxplus \left(\boldsymbol{\xi}_{c a m_{-}w}^{D}\right)^{-1} \boxplus \boldsymbol{\xi}_{m_{-} d}^{-1} \boxplus \boldsymbol{\xi}_{c a m_{-}i m u}^{M}\\ &=\log \left(\left(\mathbf{T}_{c a m_{-}i m u}^{M}\right)^{-1} \cdot \mathbf{T}_{m_{-} d} \cdot \mathbf{T}_{c a m_{-}w}^{D} \cdot \exp (\epsilon)^{-1} \cdot\left(\mathbf{T}_{c a m\_w}^{D}\right)^{-1} \cdot \mathbf{T}_{m_{-}d}^{-1} \cdot \mathbf{T}_{c a m_{-}i m u}^{M}\right) \\ &=\operatorname{Adj}\left(\left(\mathbf{T}_{c a m_{-}i m u}^{M}\right)^{-1} \cdot \mathbf{T}_{m_{-} d} \cdot \mathbf{T}_{c a m_{-} w}^{D}\right) \cdot(-\epsilon) \end{aligned} \tag{17}$
所以最後得到Jacobian相對於位姿導數公式（17）
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m\_w}^{D} \boxplus \boldsymbol{\epsilon}, \boldsymbol{\xi}_{m\_ d}\right)}{\partial \boldsymbol{\epsilon}}=-\operatorname{Adj}\left(\left(\mathbf{T}_{c a m_{-}i m u}^{M}\right)^{-1} \cdot \mathbf{T}_{m_{-}d} \cdot \mathbf{T}_{c a m_{-}w}^{D}\right) \tag{18}$
**2）**推導Jacobian相對於尺度和重力方向的導數
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m_{-} w}^{D}, \boldsymbol{\xi}_{m_{-} d} \boxplus\boldsymbol{\epsilon}\right)}{\partial \boldsymbol{\epsilon}}$
根據BCH公式：
$\log (\exp (\boldsymbol{a}) \cdot \exp (\boldsymbol{b}))=\boldsymbol{a}+\boldsymbol{b}+\frac{1}{2}[\boldsymbol{a}, \boldsymbol{b}]+\frac{1}{12}([\boldsymbol{a},[\boldsymbol{a}, \boldsymbol{b}]]+[\boldsymbol{b},[\boldsymbol{b}, \boldsymbol{a}]])+\frac{1}{48}([\boldsymbol{b},[\boldsymbol{a},[\boldsymbol{b}, \boldsymbol{a}]]]+[\boldsymbol{a},[\boldsymbol{b},[\boldsymbol{b}, \boldsymbol{a}]]])+\ldots \tag{19}$
李括號：$[a,b]=ab-ba$

同公式（17）的推導一樣湊成Adj形式：

根據公式（16）得到：
$\boldsymbol{a}=\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m\_{d}} \cdot \mathbf{T}_{c a m_{-}w}\right) \cdot \epsilon \tag{19}$

$\boldsymbol{b}=-\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m_{-}d}\right) \cdot \epsilon \tag{20}$

把公式（19）（20）帶入BCH公式，即公式（19）
$\boldsymbol{a} \cdot \boldsymbol{b}=\underbrace{\operatorname{Adj}\left(\mathbf{T}_{c a m\_ i m u}^{-1} \cdot \mathbf{T}_{m\_d} \cdot \mathbf{T}_{c a m\_ w}\right) \cdot \epsilon \cdot\left(-\operatorname{Adj}\left(\mathbf{T}_{c a m\_ i m u}^{-1} \cdot \mathbf{T}_{m\_ d}\right)\right) \cdot \epsilon}_{\mu_{1}(\epsilon)} \tag{21}$

$\boldsymbol{b} \cdot \boldsymbol{a}=\underbrace{-\operatorname{Adj}\left(\mathbf{T}_{c a m\_ i m u}^{-1} \cdot \mathbf{T}_{m\_d}\right) \cdot \boldsymbol{\epsilon} \cdot \operatorname{Adj}\left(\mathbf{T}_{c a m\_i m u}^{-1} \cdot \mathbf{T}_{m \_ d} \cdot \mathbf{T}_{c a m\_w}\right)}_{\mu_{2}(\epsilon)} \cdot \epsilon \tag{22}$

$[\boldsymbol{a}, \boldsymbol{b}]=\boldsymbol{a} \boldsymbol{b}-\boldsymbol{b} \boldsymbol{a}=\left(\mu_{1}(\boldsymbol{\epsilon})+\mu_{2}(\boldsymbol{\epsilon})\right) \cdot \boldsymbol{\epsilon}\tag{23}$

公式（23）在$\boldsymbol \epsilon \to 0$時，$\mu_1 \mu_2$爲0

所以Jacobian相對於尺度和重力方向的導數爲：
$\frac{\partial \Psi\left(\boldsymbol{\xi}_{c a m\_w}^{D}, \boldsymbol{\xi}_{m\_d} \boxplus \boldsymbol{\epsilon}\right)}{\partial \boldsymbol{\epsilon}}=\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m_{-} d} \cdot \mathbf{T}_{c a m_{-} w}\right)-\operatorname{Adj}\left(\mathbf{T}_{c a m_{-}i m u}^{-1} \cdot \mathbf{T}_{m_{-} d}\right) \tag{24}$

Reference

---

1. Supplementary Material to: Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization ↩︎