VIO學習筆記（三）—— 基於優化的 IMU 與視覺信息融合

學習資料是深藍學院的《從零開始手寫VIO》課程，對課程做一些記錄，方便自己以後查詢，如有錯誤還請斧正。由於習慣性心算公式，所以爲了加深理解，文章公式採用手寫的形式。

VIO學習筆記（一）—— 概述
VIO學習筆記（二）—— IMU 傳感器

基於 Bundle Adjustment 的 VIO 融合

視覺 SLAM 裏的 Bundle Adjustment 問題

已知:
  狀態量初始值:特徵點的三維座標,相機的位姿。
  系統測量值:特徵點在不同圖像上的圖像座標。

解決方式:
  構建誤差函數,利用最小二乘得到狀態量的最優估計:
$\argmin_{q,p,f} \sum_{i=1}^m\sum_{j=1}^n ||π (q _{wc _i} , p _{wc _i} , f _j ) − z_{ f _j}^{c_i} ||_{Σ _{ij}}$

符號定義:
  $q$: 旋轉四元數
  $p$: 平移向量
  $f$: 特徵點3D座標
  $c_i$: 第ｉ個相機系
  $π(·)$: 投影函數
  $Z_{f_j}^{c_i}$: $c_i$對$f_j$的觀測
  $\sum_{ij}$: $\sum$範數

            

g2o or ceres 中採用如下的求解方式

VIO 信息融合問題

由於imu的數據採集頻率大於相機的採集頻率，所以在兩個相機座標系之間，存在多個imu座標系即body座標系，由於相機座標系應根body座標系相對應，所以對兩圖像間的imu數據進行預積分，使其變爲一個。

預積分的簡單理解

爲什麼要預積分，slam系統中爲了減小優化求解器的負擔，採用了關鍵幀策略，IMU的速率顯然要快於關鍵幀的插入，它們之間的關係可以用下圖很好的表示。

緊耦合的方式就是把imu和圖像的信息共同來估計狀態量，所以如何協調兩者之間的關係了，預積分幹了這麼一件事，通過重新參數化，把關鍵幀之間的IMU測量值積分成相對運動的約束，避免了因爲初始條件變化造成的重複積分。

用IMU的slam、vio算法有很多，有濾波器的比如MSCKF，有基於圖優化的比如VINS，OKVIS，ORB-SLAM等。就拿ORB-SLAM來說吧， 在bundle adjustment裏，參與對象是keyframe，比如有2個keyframe： $KF_1,KF_2$, 他們的位姿分別爲：$P_{1w},P_{2w}$,那麼他們的相對位姿 :
$P_{21}=P_{1w}·P_{2w}^{-1}$
我們可以認爲$P_{21}$爲估計項，是由SLAM的位姿直接算出來的。如果要構成一個優化問題，我們還需要知道誤差項和測量項。沒錯，是IMU可以計算在這兩個KF間的測量項 $P_{imu}$，預計分乾的事情就是計算這個測量項。
$r=P_{21}·P_{imu}^{-1}$
在緊耦合的優化slam中，IMU就是提供了兩個關鍵幀的相對測量，從而構建誤差函數對關鍵幀姿態的迭代優化。當然實際應用中不會是這麼簡單的形式，這裏面要對各個變量分別求取誤差，然後求雅克比矩陣。

最小二乘問題的求解

最小二乘基礎概念

定義

找到一個 n 維的變量$x^*∈R^n$ ,使得損失函數 F (x) 取局部最小值:
$F(x)=\frac{1}{2}\sum_{i=1}^m(f_i(x))^2$
其中$f_i$是殘差函數,比如測量值和預測值之間的差,且有 m ≥ n。局部最小值指對任意 $∥x − x^∗ ∥ < δ$ 有 $F (x^∗ ) ≤ F (x)$

損失函數泰勒展開

假設損失函數 $F (x)$ 是可導並且平滑的,因此,二階泰勒展開:
$F (x + ∆x) = F (x) + J∆x + \frac12∆x ^⊤ H∆x + O (∥∆x∥ ^3)$
其中 J 和 H 分別爲損失函數 F 對變量 x 的一階導和二階導矩陣。

損失函數泰勒展開性質

忽略泰勒展開的高階項,損失函數變成了二次函數,可以輕易得到如下性質:
  如果在點 $x_s$ 處有導數爲 0 ,則稱這個點爲穩定點。
  在點 $x_s$ 處對應的 Hessian 爲 H:
    如果是正定矩陣,即它的特徵值都大於 0,則在 $x_s$ 處有 $F (x)$ 爲局部最小值;
    如果是負定矩陣,即它的特徵值都小於 0,則在 $x_s$ 處有 $F (x)$ 爲局部最大值;
    如果是不定矩陣,即它的特徵值大於 0 也有小於 0 的,則 $x_s$ 處爲鞍點。

求解法

直接求解:線性最小二乘。
迭代下降法:適用於線性和非線性最小二乘。

迭代法初衷

找一個下降方向使損失函數隨 x 的迭代逐漸減小,直到 x 收斂到 $x^∗$ :
$F (x_{k+1} ) < F (x _k )$
分兩步:
  第一,找下降方向單位向量 d,
  第二,確定下降步長 α.

假設 α 足夠小,我們可以對損失函數 F (x) 進行一階泰勒展開:
$F (x + αd) ≈ F (x) + αJd$
只需尋找下降方向,滿足:
$Jd < 0$
通過 line search 方法找到下降的步長:$α ^∗ = argmin _{α>0} {F (x + αd)}$

對損失函數泰勒展開

最速下降法(一階梯度法): 適用於迭代的開始階段

從下降方向的條件可知:$Jd = ∥J∥ cos θ$,$θ$ 表示下降方向和梯度方向的夾角。當 $θ = π$ ,有
$d =\frac{-J^T}{∥J∥}$
即梯度的負方向爲最速下降方向。缺點:最優值附近震盪,收斂慢。

牛頓法(二階梯度法):適用於最優值附近

在局部最優點 $x ^∗$ 附近,如果 $x + ∆x$ 是最優解,則損失函數對 ∆x的導數等於 0,對損失函數的二階泰勒展式取一階導有:
$\frac{∂}{∂∆x}(F (x) + J∆x + \frac12∆x ^⊤ H∆x )= J ^⊤ + H∆x = 0$
得到:$∆x = −H^{ −1} J ^⊤$ 。缺點:二階導矩陣計算複雜。

Damp Method

將損失函數的二階泰勒展開記作
$F (x + ∆x) ≈ L(∆x) ≡ F (x) + J∆x + ∆x ^⊤ H∆x$
求以下函數的最小化:
$∆x ≡ arg\min_{∆x} (L(∆x) + \frac12μ∆x^ ⊤ ∆x)$
其中,$μ ≥ 0$爲阻尼因子, $\frac12 μ∆x ^⊤ ∆x = \frac12 μ∥∆x∥^ 2$是懲罰項(不讓步長過大)。
對新的損失函數求一階導,並令其等於 0 有:
$L ^′ (∆x) + μ∆x = 0\\ ⇒ (H + μI) ∆x = −J ^⊤$

對殘差函數泰勒展開

符號說明

爲了公式約簡,可將殘差組合成向量的形式。
$\vec{f } (x)= \begin{bmatrix} f_1(x) \\ f_2(x) \\ … \\f_1(x) \\ \end{bmatrix}$
則有:　　$f ^⊤ (x)f (x) = \sum_{i=1}^m (f _i (x))^2$
同理,如果記 $\vec{J}_ i (x) = \frac{∂f_i(x)}{∂x}$ 則有:
$\frac{∂\vec{f}(x)}{∂x}= \vec{J} =\begin{bmatrix} \vec{J}_ 1 (x) \\ \vec{J}_ 2(x) \\ … \\\vec{J}_ i (x) \\ \end{bmatrix}$

基礎

殘差函數 $\vec{f} (x)$ 爲非線性函數,對其一階泰勒近似有:
$f (x + ∆x) ≈ l(∆x) ≡ f (x) + J∆x$
請特別注意,這裏的 J 是殘差函數 f 的雅克比矩陣。代入損失函數:
$F (x + ∆x) ≈ L(∆x) ≡ \frac12l(∆x) ^⊤ l(∆x)\\ \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad= \frac12f ^⊤ f + ∆x ^⊤ J ^⊤ f + \frac12∆x ^⊤ J ^⊤ J∆x\\ \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad= F (x)+ ∆x ^⊤ J ^⊤ f + \frac12∆x ^⊤ J ^⊤ J∆x$
這樣損失函數就近似成了一個二次函數,並且如果雅克比是滿秩的,則 $J ^⊤ J$ 正定,損失函數有最小值。
另外, 易得:$F ^′ (x) = (J ^⊤ f ) ^⊤$ ,以及$F ^{′′} (x) ≈ J ^⊤ J$.

Gauss-Newton Method

令損失函數就近似式的一階導等於 0,得到:
$J ^⊤ J ∆x_{ gn }= −J^ ⊤ f$
上式就是通常論文裏看到的 $H∆x _{gn} = b$,稱其爲 normal equation.

Levenberg-Marquardt Method

Levenberg (1944) 和 Marquardt (1963) 先後對高斯牛頓法進行了改進,求解過程中引入了阻尼因子:
$(J^ ⊤ J + μI )∆x_{lm} = −J ^⊤ f \quad \quad \quad with \quad μ ≥ 0$

阻尼因子的作用

$μ > 0$ 保證 $(J ^⊤ J + μI)$ 正定,迭代朝着下降方向進行。
$μ$ 非常大,則 $∆x _{lm} = −\frac 1μ J ^⊤ f = − \frac1μ F^ ′ (x) ^⊤$ , 接近最速下降法.
$μ$ 比較小,則 $∆x _{lm} ≈ ∆x _{gn}$ , 接近高斯牛頓法。

阻尼因子初始值的選取

阻尼因子 $μ$ 大小是相對於 $J ^⊤ J$ 的元素而言的。半正定的信息矩陣$J^⊤ J$特徵值 {${λ_j }$} 和對應的特徵向量爲 {${v _j }$}。對 $J ^⊤ J$ 做特徵值分解分解後有:$J ^⊤ J = VΛV ^⊤$ 可得(不知道咋算出來的，先記住接着向下走吧，有時間再補特徵分解的知識！！！):
$∆x_{ lm} = −\sum_{j=1}^n\frac{v_j^TF^{'T}}{λ_j+μ}v_j$
所以,一個簡單的 $μ _0$ 初始值的策略就是:
$μ_ 0 = τ · \max{} (J ^⊤ J)_{ ii}$
通常,按需設定 $τ ∼ [10 ^{−8} , 1]$。

阻尼因子 μ 的更新策略

1. 定性分析,直觀感受阻尼因子的更新:
     如果 $∆x → F (x) ↑$ ,則 $μ ↑→ ∆x ↓$, 增大阻尼減小步長,拒絕本次迭代。
     如果 $∆x → F (x) ↓$ ,則 $μ ↓→ ∆x ↑$, 減小阻尼增大步長。加快收斂,減少迭代次數。
2. 定量分析,阻尼因子更新策略通過比例因子來確定的:
   $ρ =\frac{F (x) − F (x + ∆x _{lm} )}{L(0) − L (∆x _{lm} )}$
   其中:
   則有：
   $ρ =\frac{F (x) − F (x + ∆x _{lm} )}{\frac12∆x_{lm}^T(μ∆x_{lm}+b)} \quad \quad \quad b=-J^Tf$

Marquardt 策略

首先比例因子分母始終大於 0,如果:
  $ρ < 0$, 則 $F (x) ↑$ ,應該 $μ ↑→ ∆x ↓$, 增大阻尼減小步長。
  如果 $ρ > 0$ 且比較大,減小 $μ$, 讓 LM 接近 Gauss-Newton 使得系統更快收斂。
  反之,如果是比較小的正數,則增大阻尼 $μ$,縮小迭代步長。
1963 年 Marquardt 提出了一個如下的阻尼策略:
$if \quad \quad\quadρ < 0.25\\ \quad\quad\quad\quadμ := μ ∗ 2\\ elseif \quadρ > 0.75\\ \quad\quad\quad\quadμ := μ/3$
Marquardt 好不好呢?如下圖所示 :
          
會看到 $μ$存在一定的跳動，這就導致步長存在一定的晃動，不穩定。

Nielsen 策略 (被 g2o, ceres 採用)

$\quad \quad if \quadρ > 0\\ \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quadμ := μ ∗ max(\frac13,1 − (2ρ − 1)^ 3 );ν := 2 \\ else\\ \quad\quad\quad\quad\quad\quad\quad\quad\quad\quadμ := μ ∗ ν;ν := 2 ∗ ν$
        

魯棒核函數的實現

引言:最小二乘中遇到 outlier 怎麼處理?核函數如何在代碼中實現?有多種方法,這裏主要介紹 g2o 和 ceres 中使用的 Triggs Correction .

Triggs Correction

魯棒核函數直接作用殘差 $f _k (x)$ 上,最小二乘函數變成了如下形式:
$\min_x\frac12\sum_kρ (∥f _k (x)∥ ^2)$
將誤差的平方項記作 $s _k = ∥f _k (x)∥^ 2$ , 則魯棒核誤差函數進行二階泰勒展開有:
$\frac12ρ (s) = \frac12(const + ρ ^′ ∆s + ρ^ ′′ ∆^ 2 s)$
上述函數中 $∆s _k$ 的計算稍微複雜一點:

$∆s _k$ 代入$\frac12ρ (s)$有:

代三角號的計算步驟很蒙，沒有算出來，找了一下論文，發現論文也是這樣寫的：
            
估計是哪個地方的知識點不到位把，再研究研究！！！

對上式求和後,對變量 ∆x 求導,令其等於 0 ,得到:

Example: Cauchy Cost Function

柯西魯棒核函數的定義爲:
$ρ(s) = c^ 2 log(1 +\frac{s}{c^2})$
其中 $c$ 爲控制參數。對 $s$ 的一階導和二階導爲:
$ρ ^′ (s) =\frac{1}{1+\frac{s}{c^2}},\quad\quad\quad\quadρ ^{′ ′} (s) =-\frac{1}{c^2}(ρ ^′ (s) )^2$

核函數拓展

核函數控制參數的設定

95% efficiency rule (Huber, 1981):itprovides an asymptotic efficiency 95% that of linear regression for the normal distribution.
• 如果殘差 $f _i$ 是正態分佈,Huber
  $c = 1.345$,
  $Cauchy c = 2.3849$
• 如果殘差非正態分佈,需估計殘差方差,然後對殘差歸一化。median absolute residual 方法
  $σ = 1.482 · med(med(r) − r _i )$.

回顧最小二乘求解

1. 找到一個合適的關於狀態量 x 的殘差函數 $f _i (x)$,後續用 r, err等表示。
2. 計算殘差函數對狀態量 x 的雅克比 J。
3. 選定 cost function 以及其參數。
4. LM 算法求解。

VIO 殘差函數的構建

視覺重投影誤差

視覺重投影誤差

定義:
  一個特徵點在歸一化相機座標系下的估計值與觀測值的差。
$r _c= \begin{bmatrix} \frac xz-u \\ \frac yz-v \\ \end{bmatrix}$
其中,待估計的狀態量爲特徵點的三維空間座標 $(x, y, z) ^⊤$ ,觀測值$(u, v) ^⊤$ 爲特徵在相機歸一化平面的座標。

逆深度參數化

特徵點在歸一化相機座標系與在相機座標系下的座標關係爲:
$\begin{bmatrix} x \\y \\z\end{bmatrix}=\frac1λ\begin{bmatrix} u \\v \\1\end{bmatrix}$
其中 $λ = 1/z$ 稱爲逆深度(更接近於高斯分佈)。

VIO 中基於逆深度的重投影誤差

        
特徵點逆深度在第 i 幀中初始化得到,在第 j 幀又被觀測到,預測其在第 j 中的座標爲:
$\begin{bmatrix} x _{c_ j} \\y _{c_ j} \\z _{c_ j} \\1\end{bmatrix}=T^{−1}_{bc}T _{wb_j}^{-1}T_{wb_i} T_{bc} \begin{bmatrix} \frac1λu_{c_i} \\\frac1λv_{c_i} \\\frac1λ\\1\end{bmatrix}$
將i幀中觀測到的數據變換到相機座標系，將相機座標系變換到body座標系，將第i個body座標系變換到世界座標系，將世界座標系變換到第j個body座標系，將body座標系變換到相機座標系，得到第j幀的預測值。這期間相對於純視覺多了相機座標系變換到body座標系，然後再由body座標系變換回相機座標系的過程。
視覺重投影誤差爲:
$r _c= \begin{bmatrix} \frac {x_{c_i}}{ z_{c_i}}-u_{c_i} \\ \frac {y_{c_i}}{ z_{c_i}}-u_{c_i} \\ \end{bmatrix}$

IMU 測量值積分

IMU 的真實值爲 $ω$, $a$, 測量值爲 $\tilde{ω}$, $ã$,則有:
$ω̃ ^b = ω^b + b^ g + n ^g\\ ã = q _{bw} (a ^w+g^w ) + b^a + n^a$
上標 $g$ 表示 gyro,$a$ 表示 acc,$w$ 表示在世界座標系 world,$b$ 表示imu 機體座標系 body。
PVQ 對時間的導數可寫成:
$ṗ _{wb _t} = v _t^ w\\ v̇ _t^ w = a_ t^ w\\ q̇ _{wb _t} = q _{wb _t}⊗\begin{bmatrix} 0\\ \frac12w^{b_t}\\ \end{bmatrix}$

測量值對世界座標的積分

從第 i 時刻的 PVQ 對 IMU 的測量值進行積分得到第 j 時刻的 PVQ:
      
$p_{wb_j} = p_{wb_i}+ v _i^ w ∆t +\iint_{t∈[i,j]}(q_{ wb _t} a ^{b t} − g ^w )δt^ 2\\ v _j ^w= v _i ^w+\int_{t∈[i,j]}(q_ {wb _t} a^{b_t} − g ^w )δt\\ q _{wb _j}=\int_{t∈[i,j]}q _{wb _t}⊗\begin{bmatrix} 0\\ \frac12w^{b_t}\\ \end{bmatrix}$
每次 q wb t 優化更新後,都需要重新進行積分,運算量較大。所以引出下文預積分解決方案。

IMU 預積分

一個很簡單的公式轉換,就可以將積分模型轉爲預積分模型:
$q_{ wb _t} = q _{wb _i} ⊗ q _{b _i b _t}$
那麼,PVQ 積分公式中的積分項則變成相對於第 i 時刻的姿態,而不是相對於世界座標系的姿態:
$p_{wb_j} = p_{wb_i}+ v _i^ w ∆t -\frac12g^w∆t ^2+q_{wb_i}\iint_{t∈[i,j]}(q _{b _i b _t} a ^{b t} )δt^ 2\\ v _j ^w= v _i ^w-g^w∆t+q_{wb_i}\int_{t∈[i,j]}(q _{b _i b _t} a ^{b t} )δt\\ q _{wb _j}=q_{wb_i}\int_{t∈[i,j]}q _{b _i b _t} ⊗\begin{bmatrix} 0\\ \frac12w^{b_t}\\ \end{bmatrix}$

預積分量

預積分量僅僅跟 IMU 測量值有關,它將一段時間內的 IMU 數據直接積分起來就得到了預積分量:
$α _{b _i b _j} =\iint_{t∈[i,j]}(q _{b _i b _t} a ^{b t} )δt^ 2\\ β _{b _i b _j} = \int_{t∈[i,j]}(q _{b _i b _t} a ^{b t} )δt\\ q _{b _i b _j}=\int_{t∈[i,j]}q _{b _i b _t} ⊗\begin{bmatrix} 0\\ \frac12w^{b_t}\\ \end{bmatrix}$
重新整理下 PVQ 的積分公式,有:
$\begin{bmatrix} p_{wb_j} \\ v _j ^w \\ q _{wb _j} \\ b_j^a \\ b_j^g \end{bmatrix} = \begin{bmatrix} p_{wb_i}+ v _i^ w ∆t -\frac12g^w∆t ^2+q_{wb_i}α _{b _i b _j} \\ v _i ^w-g^w∆t+q_{wb_i}β _{b _i b _j} \\ q_{wb_i}q _{b _i b _j} \\ b_i^a \\ b_i^g \end{bmatrix}$

預積分誤差

定義:
  一段時間內 IMU 構建的預積分量作爲測量值,對兩時刻之間的狀態量進行約束,
$\begin{bmatrix} r _p \\ r _v \\ r _q \\ r_{ba} \\ r_{bg} \end{bmatrix} _{15×1} = \begin{bmatrix} q_{b_iw}(p_{wb_j} -p_{wb_i}- v _i^ w ∆t + \frac12g^w∆t ^2)-α _{b _i b _j} \\ q_{b_iw}(v _j ^w - v _i ^w + g^w∆t) - β _{b _i b _j} \\ 2[q_{b _jb _i} ⊗ (q_{ b _i w} ⊗ q_{ wb _j} )] _{xyz} \\ b_j^a - b_i^a \\ b_j^g - b_i^g \end{bmatrix}$
上面誤差中位移,速度,偏置都是直接相減得到。第二項是關於四元數的旋轉誤差,其中 [·] xyz 表示只取四元數的虛部 (x, y, z) 組成的三維向量。

預積分的離散形式

這裏使用 mid-point 方法,即兩個相鄰時刻 k 到 k+1 的位姿是用兩個時刻的測量值 a, ω 的平均值來計算:
        

預積分量的方差

疑問:

  一個 IMU 數據作爲測量值的噪聲方差我們能夠標定。現在,一段時間內多個 IMU 數據積分形成的預積分量的方差呢?

Covariance Propagation(協方差傳播)

已知一個變量 $y = Ax, x ∈ N (0, Σ _x )$, 則有 $Σ _y = AΣ _x A ^⊤$
$Σ _y = E((Ax)(Ax)^ ⊤ )\\ = E(Axx ^⊤ A ^⊤ )\\ = AΣ x A ^⊤$
所以,要推導預積分量的協方差,我們需要知道imu 噪聲和預積分量之間的線性遞推關係。
假設已知了相鄰時刻誤差的線性傳遞方程:
$η_{ ik} = F _{k−1} η _{ik−1} + G _{k−1} n _{k−1}$
比如:狀態量誤差爲 $η_ {ik} = [δθ_{ ik} , δv _{ik} , δp _{ik} ]$,測量噪聲爲$n _k = [n _k^ g , n _k^ a ]$。
誤差的傳遞由兩部分組成:當前時刻的誤差傳遞給下一時刻,當前時刻測量噪聲傳遞給下一時刻。

一個有趣的例子

綜藝節目中常有傳遞信息的節目,前一個人根據上一個人的信息 + 自己的理解(測量)傳遞給下一個人,導致這個信息越傳越錯。
協方差矩陣可以通過遞推計算得到:
$Σ _{ik} = F _{k−1} Σ _{ik−1} F^ ⊤_{k−1} + G _{k−1} Σ _n G_{ k−1}^T$
其中,$Σ _n$ 是測量噪聲的協方差矩陣,方差從 i 時刻開始進行遞推,$Σ _{ii} = 0$。

狀態誤差線性遞推公式的推導

簡介

通常對於狀態量之間的遞推關係是非線性的方程如$x _k = f (x _{k−1} , u _{k−1})$,其中狀態量爲 $x$,$u$ 爲系統的輸入量。
我們可以用兩種方法來推導狀態誤差傳遞的線性遞推關係:
  一種是基於一階泰勒展開的誤差遞推方程。
  一種是基於誤差隨時間變化的遞推方程。

基於泰勒展開的誤差傳遞(應用於 EKF 的協方差預測)

令狀態量爲 $x = x̂ + δx$,其中,真值爲 $x̂$,誤差爲 $δx$。另外,輸入量$u$ 的噪聲爲$n$。
非線性系統 $x _k = f (x _{k−1} , u _{k−1} )$ 的狀態誤差的線性遞推關係如下:
$δx_ k = Fδx _{k−1} + Gn_{ k−1}$
其中,F 是狀態量 $x _k$ 對狀態量 $x _{k−1}$ 的雅克比矩陣,G 是狀態量 $x_ k$對輸入量 $u _{k−1}$ 的雅克比矩陣。
證明:對非線性狀態方程進行一階泰勒展開有:
$x _k = f (x _{k−1} , u _{k−1} )\\ x̂ _k + δx_ k = f (x̂_{ k−1} + δx_{ k−1} , û_{ k−1} + n _{k−1} )\\ x̂ _k + δx _k = f (x̂ _{k−1}, û _{k−1} ) + Fδx_{ k−1} + Gn _{k−1}$

基於誤差隨時間變化的遞推方程

如果我們能夠推導狀態誤差隨時間變化的導數關係,比如:
$δx^{'} =Aδx + Bn$
則誤差狀態的傳遞方程爲:
$δx _k = δx_{ k−1} + δx_{k−1} ^{'} ∆t\\ → δx _k = (I + A∆t)δx _{k−1}+ B∆tn_{k−1}$
這兩種推導方式的可以看出有:
$F = I + A∆t, G = B∆t$

第一種方法不是很好麼,爲什麼會想着去弄誤差隨時間的變化呢?

這是因爲 VIO 系統中已經知道了狀態的導數和狀態之間的轉移矩陣。如:我們已經知道速度和狀態量之間的關係:
$v̇ = Ra^ b + g$
那我們就可以推導速度的誤差和狀態誤差之間的關係,再每一項上都加上各自的誤差就有:
$v̇ + δv̇= R (I + [δθ] _× )( a ^b + δa ^b) + (g + δg)\\ δv̇= Rδa^ b + R[δθ]_ × (a ^b + δa^ b) + δg\\ δv̇= Rδa^ b - R[a^b]_ × δθ + δg$
由此就能依次類推,輕易寫出整個 A 和 B 其他方程了。

預積分的誤差遞推公式推導

首先回顧預積分的誤差遞推公式,將測量噪聲也考慮進模型:
     
確定誤差傳遞的狀態量,噪聲量,然後開始構建傳遞方程。
用前面一階泰勒展開的推導方式$δx_ k = Fδx _{k−1} + Gn_{ k−1}$,我們希望能推導出如下的形式:
            
F, G 爲兩個時刻間的協方差傳遞矩陣。
這裏我們直接給出 F, G 的最終形式,後面會對部分項進行詳細推導:
    
      
其中的係數爲:

下面對Ｆ的第三行進行推導，其他行列的推導類似：

殘差 Jacobian 的推導

視覺重投影殘差的 Jacobian

視覺殘差爲:

對於第 i 幀中的特徵點,它投影到第 j 幀相機座標系下的值爲:

拆成三維座標形式爲:

再推導各類 Jacobian 之前,爲了簡化公式,先定義如下變量:
                  
Jacobian 爲視覺誤差對兩個時刻的狀態量,外參,以及逆深度求導:
          
根據鏈式法則,Jacobian 的計算可以分兩步走:
第一步誤差對 $f _{c _j}$ 求導:
                  
第二步 f c j 對各狀態量求導:

1. 對 i 時刻的狀態量求導
   a. 對 i 時刻位移求導,可直接寫出如下:
   
   b. 對 i 時刻角度增量求導
   
   上面公式和 i 時刻角度相關的量並不多,下面爲了簡化,直接丟棄了不相關的部分
   
   Jacobian 爲
   
2. 對 j 時刻的狀態量求導
   a. 對位移求導:
                 
   b. 對角度增量求導,同上面的操作,也簡化一下公式
   
   Jacobian 爲
           
3. 對 imu 和相機之間的外參求導
   a. 對位移求導
               
   b. 對角度增量求導,由於 f c j 都和 R bc 有關,並且比較複雜,所以這次分兩部分求導
   
   第一部分 Jacobian 爲
       
   分子可寫成:
       
   那麼,第一部分的 Jacobian 爲:
       
   第二部分的 Jacobian 爲:
    
   兩個 Jacobian 相加就是視覺誤差對外參中的角度增量的最終結果。
4. 視覺誤差對特徵逆深度的求導
   

IMU 誤差相對於優化變量的 Jacobian

在求解非線性方程時,需要知道誤差 e B 對兩個關鍵幀 i, j 的狀態量p, q, v, b a , b g 的 Jacobian。

對 i, j 時刻的狀態量 p, q, v 求導還是比較直觀的,直接對誤差公式進行計算就行。但是對 i 時刻的 b ai , b gi 求導就顯得十分複雜,下面我們詳細討論。
因爲 i 時刻的 bias 相關的預積分計算是通過迭代一步一步累計遞推的,可以算但是太複雜。所以對於預積分量直接在 i 時刻的 bias 附近用一階泰勒展開來近似,而不用真的去迭代計算。
              
其中
    
表示預積分量對 i 時刻的 bias 求導。
這些雅克比根據前面討論的協方差傳遞公式,能一步步遞推得到:
                      
下面我們來討論 IMU 誤差相對於兩幀的 PVQ 的 Jacobian:
由於 r p 和 r v 的誤差形式很相近,對各狀態量求導的 Jacobian 形式也很相似,所以這裏只對 r v 的推導進行詳細介紹。

1. 對 i 時刻位移 Jacobian
                     
2. 對 i 時刻旋轉 Jacobian
       
   上式可寫爲:
         
3. 對 i 時刻速度 Jacobian:
                     
4. 對 i 時刻的加速度 bias 的 Jacobian,注意 bias 量只和預積分 β 有關:
                 

對ｒｐ的推導過程：
5. 對 i 時刻姿態求導
        
上式可化簡爲:
      
其中 [·] L 和 [·] R 爲四元數轉爲左/右旋轉矩陣的算子。
6. 角度誤差對 j 時刻姿態求導
      
7. 角度誤差對 i 時刻陀螺儀偏置 b gi
      
這公式……我傻了！！！

相關資料：

1. Kaj Madsen, Hans Bruun Nielsen, and Ole Tingleff. “Methods for non-linear . least squares problems”. In: (1999).
2. Christopher Zach. “Robust bundle adjustment revisited”. In: European Conference on Computer Vision. Springer. 2014, pp. 772–787.
3. Bill Triggs et al. “Bundle adjustment—a modern synthesis”. In: International workshop on vision algorithms. Springer.1999, pp. 298–372.
4. Kirk MacTavish and Timothy D Barfoot. “At all costs: A comparison of robust . cost functions for camera correspondence outliers”. In: 2015 12th Conference on Computer and Robot Vision. IEEE. 2015, . pp.62–69.