VINS-Mono之後端非線性優化 (目標函數中視覺殘差和IMU殘差，及其對狀態量的雅克比矩陣、協方差遞推方程的推導)

1. 前言

之前看過崔華坤的《VINS論文推導及代碼解析》還有深藍學院的VIO課程，對VINS的後端非線性優化有了較爲清晰的認識，但是一直沒有時間整理寫成筆記，最近看到Manni的博客VINS-Mono理論學習——後端非線性優化 概括得很不錯，針對這三份資料還有自己的一些理解重新整理下，感謝優秀的大佬們提供的參考資料。

2. 非線性最小二乘

儘管非線性最小二乘是很常見的問題，可參考《SLAM14講》第六章節，《多視圖幾何》附錄6，喬治梅森大學Timothy Sauer的《數值分析》(忘記第幾章了，書也沒帶回家，哭.jpg)，在我之前的博客非線性最小二乘也對Guass-Newton和LM進行了介紹，這裏簡單回顧一下，由於VINS-Mono中在視覺重投影誤差添加了魯棒核函數，因此也整理下對 殘差(也可以說是誤差，一個意思，以下內容不分殘差和誤差) 添加魯棒核函數對需要優化的狀態量增量方程的影響。

2.1 Guass-Newton 和 Levenberg-Marquardt

對於最常見的最小二乘問題有：$\underset{x}{min}F(x) = \underset{x}{min}\frac{1}{2} \left \| f(x) \right \|_{2}^{2}$ 將殘差寫成組合向量的形式：$f(x) = \begin{bmatrix} f_{1}(x) \\ \cdots \\ f_{m}(x)\end{bmatrix}$, 則$F(x)=\frac{1}{2} \left \| f(x) \right \|_{2}^{2}=\frac{1}{2} f^{T}(x)f(x)=\frac{1}{2} \sum_{i=1}^{m}(f_{i}(x))^{2}$ 同理，如果記$J_{i}(x) = \frac{\partial {f_{i}(x)}}{\partial x}$, 則有：$\frac{\partial {f(x)}}{\partial x} = J^{m \times n} = \begin{bmatrix} J_{1}(x) \\ \cdots \\ J_{m}(x)\end{bmatrix}\ , \ J_{i}(x) ^{1 \times n} = \begin{bmatrix} \frac{\partial f_{i}(x)}{\partial x_{1}} \ \frac{\partial f_{i}(x)}{\partial x_{2}}\ \ \cdots \ \frac{\partial f_{i}(x)} {\partial x_{n}}\end{bmatrix}$

對殘差函數$f(x)$進行一階泰勒展開：$f(x+\Delta x ) = f(x) + J \Delta x$, 其中$J$是殘差函數$f$的雅可比矩陣，代入損失函數(以下推導用$f$代替$f(x)$)：$F(x+\Delta x) \approx L(\Delta x) = \frac{1}{2} ( f + J \Delta x)^{T}(f + J \Delta x) = \frac{1}{2} f^{T}f + \Delta x^{T} J^{T} f + \frac{1}{2} \Delta x^{T}J^{T}J\Delta x \\ = F(x) + \Delta x^{T} J^{T} f + \frac{1}{2} \Delta x^{T}J^{T}J\Delta x \tag{1}$ 這樣損失函數就近似成了一個二次函數，如果雅可比矩陣是滿秩的(並不一定滿秩)，則$J^{T}J$正定，損失函數有最小值，且易得 $F'(x) = (J^{T}f)^{T}, \ F''(x) = J^{T}J$

Gauss-Newton Method：
令公式(1)的一階導數等於0，得到$(J^{T}J) \Delta x_{gn} = -J^{T}f$ 由於$H= J^{T}J$可能出現H矩陣奇異或者病態，此時高斯牛頓法增量的穩定性較差，導致算法不收斂。同時對於步長問題，若求出來的$Δx_{gn}$太長，則可能出現局部近似不夠準確，無法保證迭代收斂。

Levenberg-Marquardt Method (LM):
在高斯牛頓法的基礎上引入阻尼因子：$(J^{T}J + \mu I) \Delta x_{lm} = -J^{T}f, \ \ \ s.t. \ \ \mu \geq 0$ 其中$\mu$稱爲阻尼因子。

阻尼因子的作用： $\mu > 0$保證$(J^{T}J+\mu I)$正定，迭代朝着下降方向進行。
阻尼因子的初始化： $\mu_{0} = \tau \ max \begin{Bmatrix} (J^{T}J)_{ij} \end{Bmatrix}$, 按需設定 $\tau \in [10^{-8}, 1]$。
阻尼因子$\mu$的更新策略：

• 如果$\Delta x \rightarrow F(x) \uparrow$，則 $\mu \uparrow \rightarrow \Delta x \downarrow$，增大阻尼減小步長，拒絕本次迭代。
• 如果$\Delta x \rightarrow F(x) \downarrow$，則 $\mu \downarrow \rightarrow \Delta x \uparrow$，減小阻尼增加步長，減少迭代次數。

$\Delta x$和阻尼因子$\mu$的關係：
阻尼因子的更新由比例因子來確定：
$\rho = \frac{F(x) - F(x+\Delta x_{lm})}{L(0) - L(\Delta x_{lm})} = \frac{{實際下降}}{近似下降}$ 其中 $L(0)-L(\Delta x_{lm}) = - \Delta x_{lm}^{T} J^{T} f - \frac{1}{2} \Delta x_{lm}^{T}J^{T}J\Delta x \\ =-\frac{1}{2} \Delta x_{lm}^{T} (2 J^{T}f + (J^{T}J+\mu I - \mu I) \Delta x_{lm}) \\ = -\frac{1}{2} \Delta x_{lm}^{T} ( J^{T}f - \mu I\Delta x_{lm}) \\ = \frac{1}{2} \Delta x_{lm}^{T}(\mu \Delta x_{lm} - J^{T}f)$ 首先比例因子分母始終大於$0$，因爲是沿負梯度方向進行的$\Delta x_{lm}$的調整，故$L(0) > L(\Delta x_{lm})$，如果：

• $\rho<0$，則$F(x) \uparrow$，應該$\mu \uparrow \rightarrow \Delta x \downarrow$，增大阻尼減小步長。
• 如果$\rho>0$且比較大，減小$\mu$，讓LM接近Gauss-Newton使得系統更快收斂。
• 反之，如果是比較小的正數，則增大阻尼$\mu$，縮小迭代步長。

2.2 魯棒核函數下狀態量增量方程的構建

由上得 $F(x)=\frac{1}{2} \left \| f(x) \right \|_{2}^{2}=\frac{1}{2} f^{T}(x)f(x)=\frac{1}{2} \sum_{i=1}^{m}(f_{i}(x))^{2}$ 魯棒核函數直接作用於殘差$f_{i}(x)$上，則有
$\underset{x}{min} \frac{1}{2} \sum_{i} \rho (\left \| f_{i}(x) \right \|_{2}^{2}) = \underset{x}{min} \frac{1}{2} \sum_{i} \rho (f_{i}(x) ^{2})$ 將誤差的平方項記作$s_{i}=\left \|f_{i}(x) \right \|_{2}^{2}$，對魯棒核誤差函數進行二階泰勒展開有：
$\rho(s+\Delta s) \approx \rho(s) + \rho'(s) \Delta s + \frac{1}{2} \rho '' (s) \Delta^{2} s \tag{2}$

---

對於$\Delta s$的計算，我們有：
$\Delta s = \left \| f_{i}(x + \Delta x) \right \|^{2}_{2} - \left \| f_{i}(x) \right \|^{2}_{2} \approx \left \| f_{i}(x) + J_{i} \Delta x \right \|^{2}_{2} - \left \| f_{i}(x) \right \|_{2}^{2} \\ = (f_{i}(x) + J_{i} \Delta x)^{2} - f_{i}^{2}(x) = 2f^{T}_{i}(x)J_{i}\Delta x + (\Delta x)^{T}J^{T}_{i}J_{i}\Delta x \tag{3.1}$
若誤差項爲$\Sigma$範數(表徵爲誤差項的協方差矩陣，協方差爲對稱矩陣，故協方差的逆(稱爲信息矩陣)亦爲對稱矩陣)而非二範數，我們有：
$\Delta s = \left \| f_{i}(x + \Delta x) \right \|^{2}_{\Sigma_{i}} - \left \| f_{i}(x) \right \|^{2}_{\Sigma_{i}} \approx \left \| f_{i}(x) + J_{i} \Delta x \right \|^{2}_{\Sigma_{i}} - \left \| f_{i}(x) \right \|_{\Sigma_{i}}^{2} \\ = (f_{i}(x) + J_{i} \Delta x)^{T}\Sigma_{i}^{-1}(f_{i}(x) + J_{i} \Delta x) - f_{i}^{T}(x)\Sigma_{i}^{-1}f_{i}(x) \\ = (\Sigma_{i}^{-T}f_{i}(x))^{T}J_{i}\Delta x + (J_{i}\Delta x)^{T}\Sigma_{i}^{-1}f_{i}(x) + \Delta x^{T}J_{i}^{T}\Sigma^{-1}_{i}J_{i}\Delta x \\ = 2f^{T}_{i}(x)\Sigma_{i}^{-1}J_{i}\Delta x + \Delta x^{T}J_{i}^{T}\Sigma^{-1}_{i}J_{i}\Delta x \tag{3.2}$

---

將公式$(3)$代入公式$(2)$，令$f_{i} (x)= f_{i}$ ，$\rho = \rho(s)$，可得
$\frac{1}{2} \rho(s+\Delta s) \approx \frac{1}{2} (\rho(s) + \rho'(s) \Delta s + \frac{1}{2} \rho '' (s) \Delta^{2} s )\\ = \frac{1}{2}(\rho + \rho'[2f^{T}_{i}J_{i}\Delta x + (\Delta x)^{T}J^{T}_{i}J_{i}\Delta x]+ \frac{1}{2} \rho ''[2f^{T}_{i}J_{i}\Delta x + (\Delta x)^{T}J^{T}_{i}J_{i}\Delta x]^{2}) \\ \approx \rho + \rho'f_{i}^{T}J_{i}\Delta x + \frac{1}{2} \rho'(\Delta x)^{T}J_{i}^{T}J_{i}^{T}\Delta x + \rho ''(\Delta x)^{T}J_{i}^{T}f_{i}f_{i}^{T}J_{i}\Delta x \\ = \rho + \rho ' f_{i}^{T}J_{i}\Delta x + \frac{1}{2} (\Delta x)^{T}J_{i}^{T}(\rho ' I + 2 \rho '' f_{i}f_{i}^{T})J_{i}\Delta x \tag{4.1}$
若誤差項爲$\Sigma$範數：
$\frac{1}{2} \rho(s+\Delta s) \approx \frac{1}{2} (\rho(s) + \rho'(s) \Delta s + \frac{1}{2} \rho '' (s) \Delta^{2} s )\\ = \frac{1}{2}(\rho + \rho'[2f^{T}_{i}\Sigma_{i}^{-1}J_{i}\Delta x + (\Delta x)^{T}J^{T}_{i}\Sigma_{i}^{-1}J_{i}\Delta x]+ \frac{1}{2} \rho ''[2f^{T}_{i}\Sigma_{i}^{-1}J_{i}\Delta x + (\Delta x)^{T}J^{T}_{i}\Sigma_{i}^{-1}J_{i}\Delta x]^{2}) \\ \approx \rho + \rho'f_{i}^{T}\Sigma_{i}^{-1}J_{i}\Delta x + \frac{1}{2} \rho'(\Delta x)^{T}J_{i}^{T}\Sigma_{i}^{-1}J_{i}\Delta x + \rho ''(\Delta x)^{T}J_{i}^{T}\Sigma_{i}^{-1}f_{i}f_{i}^{T}\Sigma_{i}^{-1}J_{i}\Delta x \\ = \rho + \rho ' f_{i}^{T}\Sigma_{i}^{-1}J_{i}\Delta x + \frac{1}{2} (\Delta x)^{T}J_{i}^{T}(\rho ' \Sigma_{i}^{-1} + 2 \rho '' \Sigma_{i}^{-1}f_{i}f_{i}^{T}\Sigma_{i}^{-1})J_{i}\Delta x \tag{4.2}$

---

由於$\Delta x$是一個極小量，故其三階及以上的項數值很小，近似忽略$\Delta x$三階及其以上的項。
對公式$(4)$中的$\Delta x$進行求導後，令其等於0，得到： $\sum_{i} J_{i}^{T}(\rho ' I+2 \rho '' f_{i}f_{i}^{T})J_{i} \Delta x = -\sum_{i}\rho ' J_{i}^{T}f_{i}$ 化簡得：$\sum_{i} J_{i}^{T} W J_{i}\Delta x = -\sum_{i}\rho ' J_{i}^{T}f_{i}$ 其中 $W=\rho ' I+2 \rho '' f_{i}f_{i}^{T}$

若誤差項爲$\Sigma$範數：
$\sum_{i} J_{i}^{T}(\rho ' \Sigma_{i}^{-1}+2 \rho '' \Sigma_{i}^{-1}f_{i}f_{i}^{T}\Sigma_{i}^{-1})J_{i} \Delta x = -\sum_{i}\rho ' J_{i}^{T}\Sigma_{i}^{-1}f_{i}$ 化簡得：$\sum_{i} J_{i}^{T} W J_{i}\Delta x = -\sum_{i}\rho ' J_{i}^{T}\Sigma_{i}^{-1}f_{i}$ 其中 $W=\rho ' \Sigma_{i}^{-1}+2 \rho '' \Sigma_{i}^{-1}f_{i}f_{i}^{T}\Sigma_{i}^{-1}$

---

3. 局部Bundle Adjustment代價函數的構建

對於需要優化的狀態向量，包括滑動窗口內的所有IMU狀態$\mathbf{x}_{k}$(位姿$P$、速度$V$、旋轉$Q$、加速度偏置$b_{a}$，陀螺儀偏置$b_{w}$)、相機到IMU的外參 $\mathbf{x}_{c}^{b}$、所有3D點的逆深度 $\lambda_{m}$：【論文公式(21)】$\mathcal{X} = [\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots , \mathbf{x}_{n}, \mathbf{x}_{c}^{b}, \mathbf{\lambda}_{0}, \mathbf{\lambda}_{1}, \cdots, \mathbf{\lambda}_{m}]$ $\mathbf{x}_{k} = [\mathbf{p}_{b_{k}}^{w}, \mathbf{v}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}, \mathbf{b}_{a}, \mathbf{b}_{g}], k\in[0,n]$ $\mathbf{x}_{c}^{b} = [\mathbf{p}_{c}^{b}, \mathbf{q}_{c}^{b}]$ 其中$\mathbf{x}_{k}$代表相機獲取圖片時對應時刻$k$的IMU狀態，$n$爲滑動窗口內的關鍵幀的數量，$m$爲滑動窗口內3D視覺特徵(空間點)的數量，$\lambda_{m}$爲第$m$個3D視覺特徵的逆深度(第一次被觀察時計算得到的值)。
目標函數爲 【論文公式(22)(23)】：$\underset{\mathbf{\mathcal{X}}}{min}\begin{Bmatrix}\left \| \mathbf{r}_{p}-\mathbf{H}_{p} \mathbf{\mathcal{X}} \right \|^{2} + \underset{k \in \mathcal{B}}{\sum} \left \| \mathbf{r}_{\mathcal{B}}(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathbf{\mathcal{X}} )\right \| _{\mathbf{P}_{b_{k+1}}^{b_{k}}} ^{2} + \underset{(l,j)\in \mathcal{C}}{\sum} \rho(\left \| \mathbf{r}_{\mathcal{C}}(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathbf{\mathcal{X}})\right \|_{\mathbf{P}_{l}^{c_{j}}}^{2}) \end{Bmatrix} \tag{5}$ $\rho(s)=\left\{\begin{matrix} 1, s\geq 1\\ 2\sqrt{s}-1, s<1 \end{matrix}\right.$ 其中這三項分別爲邊緣化的先驗信息 (由滑動窗口產生的關鍵幀邊緣化)、IMU的測量誤差、視覺的重投影誤差，三種殘差都是用馬氏距離表示。$\mathbf{P}_{b_{k+1}}^{b_{k}}$代表IMU殘差的協方差，$\mathbf{P}_{l}^{c_{j}}$代表視覺重投影誤差的協方差，通過協方差的逆 (即信息矩陣) 對各個變量計算殘差時進行加權。$\rho(s)$爲Huber核函數。

4. IMU測量殘差 (增量誤差) 及其對狀態量雅克比矩陣、協方差遞推方程的推導

4.1 IMU測量殘差 (增量誤差)

在IMU預積分中我們已經得到了：
$R^{b_{k}}_{w}p^{w}_{b_{k+1}}=R^{b_{k}}_{w}(p_{b_{k}}^{w}+v^{w}_{b_{k}}\Delta t_{k}-\frac{1}{2}g^{w}\Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}}$ $R^{b_{k}}_{w}v_{b_{k+1}}^{w} = R_{w}^{b_{k}}(v_{b_{k}}^{w}-g^{w}\Delta t_{k})+\beta^{b_{k}}_{b_{k+1}}$ $q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w} = \gamma _{b_{k+1}} ^{b_{k}}$
因此倆幀之間的PVQ和bias的變化量(增量)誤差爲：【論文公式[24]】
$r_{\mathcal{B}}^{15 \times 1}(\hat{z}_{b_{k+1}}^{b_{k}},\mathcal{X}) = \begin{bmatrix} \delta \alpha_{b_{k+1}}^{b_{k}} \\ \delta \beta_{b_{k+1}}^{b_{k}} \\ \delta \theta_{b_{k+1}}^{b_{k}} \\ \delta b_{a} \\ \delta b_{w} \end{bmatrix} = \begin{bmatrix} R^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2}) - \alpha_{b_{k+1}}^{b_{k}} \\ R^{b_{k}}_{w}(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w}\Delta t_{k}) - \beta ^{b_{k}}_{b_{k+1}} \\ 2[ (\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}})^{-1} \otimes q^{w}_{b_{k+1}} ]_{xyz} \\ b_{ab_{k+1}}-b_{ab_{k}} \\ b_{wb_{k+1}}-b_{wb_{k}}\end{bmatrix} \tag{6}$
其中$[\cdot]_{xyz}$代表取四元素虛部部分來表示狀態誤差，$[\alpha_{b_{k+1}}^{b_{k}}, \beta_{b_{k+1}}^{b_{k}}, \gamma_{b_{k+1}}^{b_{k}}]^{T}$爲IMU的預積分量，具體推導可見上一篇博客VINS-Mono之IMU預積分，預積分對應的誤差、協方差及雅克比矩陣遞推方程的推導，同時，加速度計和陀螺儀的偏置同樣包含在殘差中以在線校正。

4.2 IMU優化變量

優化變量主要包含第$k$時刻、第$k+1$時刻(其中$k$和$k+1$爲前後捕獲倆幀圖像對應的時間)的PVQ和加速度計的偏置$b_{a}$、陀螺儀的偏置$b_{w}$:
$[p_{b_{k}}^{w}, q_{b_{k}}], [v_{b_{k}}^{w}, b_{a_{k}}, b_{w_{k}}], [p_{b_{k+1}}^{w}, q_{b_{k+1}}^{w}], [v_{b_{k+1}}^{w}, b_{a_{k+1}}, b_{w_{k+1}}]$

4.3 IMU增量殘差對狀態量的雅克比矩陣

這部分在imu_factor.h中的class IMUFactor:public ceres::SizedCostFunction<15,7,9,7,9>的函數virtual bool Evaluate()中實現，其中parameters[0~3]分別對應了以上4組優化變量的參數塊，4組參數的長度依次是7,9,7,9。

代碼IMUFactor::Evaluate()中redidual還乘以一個信息矩陣sqrt_info，這是由於在IMU測量誤差和視覺的重投影誤差通過協方差的逆進行了加權，因此實際優化的是$min(d) = min(r_{k}^{T}P^{-1}r_{k})$, 這裏$r_{k}$代表$k$時刻時候的殘差，$P$代表協方差。而Ceres只接受諸如$min(r_{k}^{T}r_{k})$的優化，因此在代碼裏，需要將信息矩陣$P^{-1}$做$LLT$分解，即$LL^{T} = P^{-1}$ ,代碼中矩陣L對應sqrt_info,這樣就可以將優化進行轉換：$min(d) = min(r_{k}^{T}P^{-1}r_{k}) = min ((L^{T}r_{k})^{T}(L^{T}r_{k})) = min(r_{k}'^{T} r_{k})$

計算雅克比時，殘差對應的求偏差對象爲上面的優化變量，但是計算時採用擾動方式計算，即擾動爲$[\delta p_{b_{k}}^{w}, \delta \theta_{b_{k}}^{w}], [v_{b_{k}}^{w}, \delta b_{a_{k}}, \delta b_{w_{k}}], [\delta p_{b_{k+1}}^{w}, \delta \theta_{b_{k+1}}^{w}], [\delta v_{k+1}^{w}, \delta a_{k+1}, \delta b_{w_{k+1}}]$

這裏給出先給出雅克比$J$的結果(對應上面的parameters[3])
$J[0]^{15 \times 7} = [\frac{\partial{r_{\mathcal{B}}}}{\partial{p_{b_{k}}^{w}}}, \frac{\partial{r_{\mathcal{B}}}}{\partial{q_{b_{k}}^{w}}}] = \begin{bmatrix} -R_{w}^{b_{k}} & [R_{w}^{b_{k}}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2}g^{w}\Delta t_{k}^{2})]_{\times} \\ 0 & [R_{w}^{b_{k}}(v_{b_{k+1}}^{w} - v_{b_{k}}^{w} + g^{w}\Delta t _{k})]_{\times} \\ 0 & -\mathcal{L}_{3 \times 3}[(q^{w}_{k+1})^{-1} \otimes q_{b_{k}}^{w}] \mathcal{R}_{3 \times 3}[\gamma_{b_{k+1}}^{b_{k}}] \\ 0 & 0 \\ 0 & 0\end{bmatrix} \tag{7}$ $J[1]^{15 \times9} = [\frac{\partial{r_{\mathcal{B}}}}{\partial{v_{b_{k}}^{w}}}, \frac{\partial{r_{\mathcal{B}}}}{\partial{b_{a_{k}}}}, \frac{\partial{r_{\mathcal{B}}}}{\partial{b_{w_{k}}}}] = \begin{bmatrix} -R_{w}^{b_{k}}\Delta t_{k} & -J_{b_{a}}^{\alpha} & -J_{b_{w}}^{\alpha} \\ -R_{w}^{b_{k}} & -J_{b_{a}}^{\beta} & -J_{b_{w}}^{\beta}\\ 0 & 0 & -\mathcal{L}_{3 \times 3}[(q_{b_{k+1}}^{w})^{-1} \otimes q_{b_{k}}^{w} \otimes \gamma_{b_{k+1}}^{b_{k}}]J_{b_{w}}^{\gamma}\\0 & -I & 0 \\ 0 & 0 & -I \end{bmatrix} \tag{8}$
$J[2]^{15 \times 7} = [\frac{\partial{r_{\mathcal{B}}}}{\partial{p_{b_{k+1}}^{w}}}, \frac{\partial{r_{\mathcal{B}}}}{\partial{q_{b_{k+1}}^{w}}}] = \begin{bmatrix} R_{w}^{b_{k}} & 0 \\ 0 & 0 \\ 0 & \mathcal{L}_{3 \times 3}[(\gamma_{b_{k+1}}^{b_{k}})^{-1}\otimes (q_{b_{k}}^{w})^{-1} \otimes q_{b_{k+1}}^{w}] \\ 0 & 0 \\ 0 & 0\end{bmatrix} \tag{9}$
$J[3]^{15 \times 9} = [\frac{\partial{r_{\mathcal{B}}}}{\partial{v_{b_{k+1}}^{w}}}, \frac{\partial{r_{\mathcal{B}}}}{\partial{b_{a_{k+1}}}}, \frac{\partial{r_{\mathcal{B}}}}{\partial{b_{w_{k+1}}}}] = \begin{bmatrix} 0 & 0 & 0 \\ R_{w}^{b_{k}} & 0 & 0\\ 0 & 0 & 0\\ 0 & I & 0\\ 0 & 0 & I \end{bmatrix} \tag{10}$

對於上式，PVQ的求導可以直接採用對誤差增量進行計算，而對$b_{a}$,$b_{g}$的求導，因爲$k$時刻的bias相關的預計分計算是通過迭代一步一步遞推的，直接求導太複雜，這裏直接對預積分量在$k$時刻的bias附近用一階泰勒展開來近似，而不是真的取迭代計算:
$\alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \alpha} \delta b_{a} + J_{b_{w}}^{ \alpha} \delta b_{w}$ $\beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \beta} \delta b_{a} + J_{b_{w}}^{ \beta} \delta b_{w}$ $\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w} \end{bmatrix}$
其中$J^{\alpha}_{b_{a}} = \frac{\partial \alpha_{b_{k+1}}^{b_{k}}}{\partial \delta b_{a}}, J^{\alpha}_{b_{w}} = \frac{\partial \alpha_{b_{k+1}}^{b_{k}}}{\partial \delta b_{w}},J^{\beta}_{b_{a}} = \frac{\partial \beta_{b_{k+1}}^{b_{k}}}{\partial \delta b_{a}},J^{\beta}_{b_{w}} = \frac{\partial \beta_{b_{k+1}}^{b_{k}}}{\partial \delta b_{w}},J^{\gamma}_{b_{w}} = \frac{\partial \gamma_{b_{k+1}}^{b_{k}}}{\partial \delta b_{w}}$表示預積分量對$k$時刻的偏置的求導。關於偏置的雅克比矩陣可以根據IMU預積分討論(VINS-Mono之IMU預積分，預積分對應的誤差、協方差及雅克比矩陣遞推方程的推導)的協方差遞推公式，一步步遞推獲得。遞推公式爲：$J_{k+1}​=FJ_{k}, \ J_{0}​=I$

---

以下是對公式$(7)(8)(9)(10)$的推導：

由於$\delta \alpha_{b_{k+1}}^{b_{k}}$(預積分Pose誤差)和$\delta \beta_{b_{k+1}}^{b_{k}}$(預積分Velocity誤差)形式很接近，因此其對各個狀態量求導的雅克比形式也很相似。

首先明確下反對稱符號的性質：有$R[\delta \theta]_{\times} = -[\delta \theta]_{\times} R$ $[\delta \theta]_{\times}(Rp) = -[Rp]_{\times}\delta \theta$ $(Rexp([\delta \theta]_{\times}))^{-1} = exp(-[\delta \theta]_{\times})R^{-1}$ $(exp([\delta \theta]_{\times})R)^{-1}=R^{-1}exp(-[\delta \theta]_{\times})$

1) 預積分Pose誤差 $\delta \alpha_{b_{k+1}}^{b_{k}}$ 對$k$ 時刻狀態量雅克比進行推導 ($k+1$時刻的雅克比同理)：

• 對$k$時刻Pose $p_{b_{k}}^{w}$的導數：$\frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial p^{w}_{b_{k}}} = -R_{w}^{b_{k}}$
• 對$k$時刻Velocity $v_{b_{k}}^{w}$的導數：$\frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial v^{w}_{b_{k}}} = -R_{w}^{b_{k}}\Delta t_{k}$
• 對$k$時刻旋轉 $q_{b_{k}}^{w}$的導數：
  $\frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k}}} =\frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial \delta \theta_{b_{k}}^{w}} = \frac{\partial q^{b_{k}}_{w} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k}}^{w}\end{bmatrix} (p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2})}{\partial \delta \theta_{b_{k}}^{w}} \\= \frac{\partial R^{b_{k}}_{w}exp([\delta \theta_{b_{k}}^{w}]_{\times})(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2})}{\partial \delta \theta_{b_{k}}^{w}} \\ \approx \frac{\partial R^{b_{k}}_{w}(I+[\delta \theta_{b_{k}}^{w}]_{\times})(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2})}{\partial \delta \theta_{b_{k}}^{w}} \\ = \frac{\partial -[\delta \theta_{b_{k}}^{w}]_{\times}R_{w}^{b_{k}}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2})}{\partial \delta \theta_{b_{k}}^{w}} \\ = \frac{\partial [R_{w}^{b_{k}}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2})]_{\times} \delta \theta_{b_{k}}^{w}}{\partial \delta \theta_{b_{k}}^{w}} \\ = [R_{w}^{b_{k}}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t_{k} + \frac{1}{2}g^{w} \Delta t_{k} ^{2})]_{\times}$
• 對$k$時刻偏置$b_{a}$和$b_{w}$的導數：$\frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial b_{a}} = \frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial \alpha^{b_{k}}_{b_{k+1}}}\frac{\partial \alpha_{b_{k+1}}^{b_{k}}}{\partial b_{a}} = - J^{\alpha}_{b_{a}} \\ \frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial b_{w}} = \frac{\partial \delta \alpha_{b_{k+1}}^{b_{k}}}{\partial \alpha^{b_{k}}_{b_{k+1}}}\frac{\partial \alpha_{b_{k+1}}^{b_{k}}}{\partial b_{w}} = - J^{\alpha}_{b_{w}}$

2) 預積分Velocity誤差 $\delta \beta_{b_{k+1}}^{b_{k}}$ 對$k$ 時刻狀態量雅克比的推導同Pose誤差$\delta \alpha_{b_{k+1}}^{b_{k}}$對狀態量雅克比的推導。
3) 預積分旋轉角度誤差 $\delta \theta_{b_{k+1}}^{b_{k}}$ 對$k$ 時刻和$k+1$時刻狀態量雅克比進行推導：

• 對$k$時刻旋轉 $q_{b_{k}}^{w}$的導數： $\frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k}}} = \frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial \delta \theta_{b_{k}}} = \frac{\partial 2[ (\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}})^{-1} \otimes q^{w}_{b_{k+1}} ]_{xyz}}{\partial \delta \theta_{b_{k}}} \\ \frac{\partial 2[ (\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k}}\end{bmatrix})^{-1} \otimes q^{w}_{b_{k+1}} ]_{xyz}}{\partial \delta \theta_{b_{k}}} \\= \frac{\partial -2[ ((\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k}}\end{bmatrix})^{-1} \otimes q^{w}_{b_{k+1}})^{-1}]_{xyz}}{\partial \delta \theta_{b_{k}}} \\ = \frac{\partial -2[ (q^{w}_{b_{k+1}})^{-1} \otimes (q^{w}_{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k}}\end{bmatrix}) \otimes \gamma_{b_{k+1}}^{b_{k}}]_{xyz} }{\partial \delta \theta_{b_{k}}} \\ = -2[\mathbf{0} \ \mathbf{I}]_{3 \times 4}\frac{ \partial \mathcal{L}((q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}}) \mathcal{R}(\gamma_{b_{k+1}}^{b_{k}}) \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k}}\end{bmatrix}}{\partial \delta \theta_{b_{k}}} \\ = -2[\mathbf{0} \ \mathbf{I}]_{3 \times 4}\partial \mathcal{L}((q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}}) \mathcal{R}(\gamma_{b_{k+1}}^{b_{k}}) \frac{1}{2} \begin{bmatrix} \frac{\partial 2}{\partial \delta \theta_{b_{k}}^{x}} & \frac{\partial 2}{\partial \delta \theta_{b_{k}}^{x}} & \frac{\partial 2}{\partial \delta \theta_{b_{k}}^{z}} \\ \frac{\partial \delta \theta_{b_{k}}^{x}}{\partial \delta \theta_{b_{k}}^{y}} & \frac{\partial \delta \theta_{b_{k}}^{x}}{\partial \delta \theta_{b_{k}}^{y}} & \frac{\partial \delta \theta_{b_{k}}^{x}}{\partial \delta \theta_{b_{k}}^{z}} \\ \frac{\partial \delta \theta_{b_{k}}^{y}}{\partial \delta \theta_{b_{k}}^{x}} & \frac{\partial \delta \theta_{b_{k}}^{y}}{\partial \delta \theta_{b_{k}}^{y}} & \frac{\partial \delta \theta_{b_{k}}^{y}}{\partial \delta \theta_{b_{k}}^{z}} \\ \frac{\partial \delta \theta_{b_{k}}^{z}}{\partial \delta \theta_{b_{k}}^{x}} & \frac{\partial \delta \theta_{b_{k}}^{z}}{\partial \delta \theta_{b_{k}}^{y}} & \frac{\partial \delta \theta_{b_{k}}^{z}}{\partial \delta \theta_{b_{k}}^{z}}\end{bmatrix} \\=-2[\mathbf{0} \ \mathbf{I}]_{3 \times 4}\partial \mathcal{L}((q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}}) \mathcal{R}(\gamma_{b_{k+1}}^{b_{k}}) \frac{1}{2} \begin{bmatrix} 0& 0&0 \\ 1 &0 & 0\\ 0& 1 & 0\\ 0& 0& 1\end{bmatrix}\\= -2[\mathbf{0} \ \mathbf{I}]_{3 \times 4} \mathcal{L}((q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}}) \mathcal{R}(\gamma_{b_{k+1}}^{b_{k}})\begin{bmatrix} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{bmatrix}_{4 \times 3} \tag{*}$ $\frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k}}} = -\mathcal{L_{3 \times 3}}((q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}}) \mathcal{R_{3 \times 3}}(\gamma_{b_{k+1}}^{b_{k}})$
• 對$k$時刻陀螺儀偏置$b_{w_{k}}$的導數： $\frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k}}} = \frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial \delta b_{w_{k}}} = \frac{\partial 2[ (\gamma_{b_{k+1}}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w_{k}} \end{bmatrix})^{-1} \otimes (q^{w}_{b_{k}})^{-1} \otimes q^{w}_{b_{k+1}} ]_{xyz}}{\partial \delta b_{w_{k}}} \\ = \frac{\partial -2[ ((\gamma_{b_{k+1}}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w_{k}} \end{bmatrix})^{-1} \otimes q^{w}_{b_{k}})^{-1} \otimes q^{w}_{b_{k+1}})^{-1} ]_{xyz}}{\partial \delta b_{w_{k}}} \\ = \frac{\partial -2[ (q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}} \otimes (\gamma_{b_{k+1}}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w_{k}} \end{bmatrix})]_{xyz}}{\partial \delta b_{w_{k}}} \\ = -2[\mathbf{0} \ \mathbf{I}]_{3 \times 4}\mathcal{L}([(q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}} \otimes \gamma_{b_{k+1}}^{b_{k}})\begin{bmatrix} \mathbf{0} \\ \frac{1}{2}J_{b_{w}}^{\gamma} \end{bmatrix}_{4 \times 3} \tag{*}$ $\frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k}}} = -\mathcal{L}_{3 \times 3}([(q^{w}_{b_{k+1}})^{-1} \otimes q^{w}_{b_{k}} \otimes \gamma_{b_{k+1}}^{b_{k}}) J_{b_{w}}^{\gamma}$
• 對$k+1$時刻陀螺儀旋轉$q^{w}_{b_{k+1}}$的導數：$\frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k+1}}} = \frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial \delta \theta_{b_{k}}} = \frac{\partial 2[ (\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}} )^{-1} \otimes q^{w}_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k+1}}\end{bmatrix} ]_{xyz}}{\partial \delta \theta_{b_{k}}} \\= \frac{\partial 2[ \mathcal{L}((\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}} )^{-1} \otimes q^{w}_{b_{k+1}} ) \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\delta \theta_{b_{k+1}}\end{bmatrix} ]_{xyz}}{\partial \delta \theta_{b_{k}}} \\=2[\mathbf{0} \ \mathbf{I}]_{3 \times 4} \mathcal{L}((\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}} )^{-1} \otimes q^{w}_{b_{k+1}} ) \begin{bmatrix} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{bmatrix}_{4 \times 3} \tag{*}$ $\frac{\partial \delta \theta_{b_{k+1}}^{b_{k}}}{\partial q^{w}_{b_{k+1}}} = \mathcal{L}_{3 \times 3}((\gamma_{b_{k+1}}^{b_{k}})^{-1} \otimes (q^{w}_{b_{k}} )^{-1} \otimes q^{w}_{b_{k+1}} )$

4) 加速度計和陀螺儀偏置誤差$\delta b_{a}$, $\delta b_{w}$對$k$ 時刻和$k+1$時刻的偏置$b_{a_{b_{k}}}$, $b_{a_{b_{k+1}}}$, $b_{w_{b_{k}}}$, $b_{w_{b_{k+1}}}$雅克比分別爲: $\frac{\partial b_{a}}{b_{a_{b_{k}}}} = -I, \ \frac{\partial b_{a}}{b_{a_{b_{k+1}}}} = I, \ \frac{\partial b_{w}}{b_{w_{b_{k}}}} = -I, \ \frac{\partial b_{w}}{b_{w_{b_{k+1}}}} = I$
至此IMU預積分誤差對各個狀態量的雅克比矩陣已經推導完畢，推導過程中規中矩，需要對哪個變量進行求導，則對其添加擾動。但是公式標有(*)都用到了四元素公式的一些推導，需要熟悉這些運算，具體可看《Quaternion-kinematics-for-the-error-state-Kalman-filter》這本書。

值得注意的是，當Pose 誤差$\delta \alpha_{b_{k+1}}^{b_{k}}$和Velocity 誤差 $\delta \beta_{b_{k+1}}^{b_{k}}$對偏置進行求導時，需要用到預積分量$\alpha_{b_{k+1}}^{b_{k}}$, $\beta_{b_{k+1}}^{k}$ 與偏置$b_{a}$, $b_{w}$的遞推關係，而角度誤差$\delta \gamma_{b_{k+1}}^{k}$對偏置求導時，只用了預積分量$\gamma_{b_{k+1}}^{b_{k}}$與$b_{w}$的遞推關係。

---

4.4 協方差遞推方程

協方差即爲IMU預積分中迭代得到的IMU增量誤差的協方差。
對IMU增量誤差遞推方程：
$\delta z_{k+1}^{15 \times 1} = F^{15 \times 15} \delta z_{k}^{15 \times 1} + V^{15 \times 18} n^{18 \times 1}$ 協方差的迭代公式爲（協方差的初始值$P_{0} = 0$）：
$P_{k+1} = FP_{k}F^{T} + VQV^{T}$ 其中噪聲項的對角協方差矩陣$Q$爲：$Q^{18 \times 18} = (\sigma_{a}^{2}, \sigma_{w}^{2},\sigma_{a}^{2},\sigma_{w}^{2},\sigma_{ba}^{2},\sigma_{bw}^{2})$ 。
雅克比矩陣的迭代公式爲（雅克比矩陣的初始值爲$J_{0} =I$）： $J_{k+1} = FJ_{k}$ 這裏計算出來的$J_{k+1}$只是爲了給後面提供對 bias 的雅克比矩陣。

5. 視覺測量殘差（重投影誤差）及其對狀態量雅克比、協方差的推導

5.1 視覺測量殘差（重投影誤差）

視覺殘差是重投影誤差，對於第$l$個路標點$P$，將$P$從第一次觀看到它的第$i$個相機座標系，轉換到當前的第$j$個座標系下的歸一化平面，其座標(u, v)的誤差。以下公式根據$l, i, j$來定義。

重投影誤差：一個特徵點在歸一化相機座標系下的估計值與觀測值的差：$r_{c} = \begin{bmatrix} \frac{x}{z} - u \\ \frac{y}{z} - v\end{bmatrix}$ 其中，待估計狀態量爲特徵點的三維空間座標$(x, y, z)^{T}$，觀測值$(u, v)^{T}$ 爲特徵在相機歸一化平面的座標。

逆深度參數化：採用逆深度$\lambda$來表示特徵點在歸一化相機座標系的座標：$\begin{bmatrix} x \\ y \\ z \end{bmatrix} = \frac{1}{\lambda}\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}$ 以逆深度作爲參數進行優化的原因：一是一些觀測到的特徵點深度值可能會非常大，難以進行優化；二是可以減少實際優化的參數變量；三是逆深度更加服從高斯分佈。
對於第$l$個路標點$P$，其在第$i$幀中被第一次觀測到，其歸一化相機座標系的座標$\begin{bmatrix} u_{c_{i}}\\ v_{c_{i}} \\ 1\end{bmatrix}$
轉換到第$j$幀的座標爲：$\begin{bmatrix} u_{c_{j}}\\ v_{c_{j}} \\ 1 \end{bmatrix} = \pi{(T_{bc}^{-1}T_{wb_{j}}^{-1}T_{wb_{i}}T_{bc} \begin{bmatrix} \frac{1}{\lambda} u_{c_{i}} \\ \frac{1}{\lambda} v_{c_{i}} \\ \frac{1}{\lambda} \\ 1\end{bmatrix})}$ 上式即爲重投影方程，這裏優化的狀態量是IMU body 座標系的位姿$T_{w_{b_{j}}}, T_{w_{b_{i}}}$，因此相較於視覺SLAM的重投影，還需要加上倆項從相機到IMU body座標系的外參。

將重投影方程拆成三維座標形式： $\mathcal{P}_{l}^{c_{j}} = \begin{bmatrix} x_{c_{j}}\\ y_{c_{j}} \\ z_{c_{j}} \end{bmatrix} =R_{b}^{c}\begin{Bmatrix} R_{w}^{b_{j}}[R_{b_{i}}^{w}(R_{c}^{b} \frac{1}{\lambda} \begin{bmatrix} u_{c_{i}} \\ v_{c_{i}}\end{bmatrix} + p_{c}^{b})+p_{b_{i}}^{w}]+p_{w}^{b_{j}}\end{Bmatrix}+p_{b}^{c}$
對於變換矩陣有$R_{w}^{b_{j}} = (R_{b_{j}}^{w})^{T}, p^{b_{j}}_{w} = -(R_{b_{j}}^{w})^{T}p_{b_{j}}^{w}, R_{b}^{c} = (R_{c}^{b})^{T}, p^{c}_{b} = -(R_{c}^{b})^{T}p_{c}^{b}$，故有：
$\mathcal{P}_{l}^{c_{j}} = \begin{bmatrix} x_{c_{j}}\\ y_{c_{j}} \\ z_{c_{j}} \end{bmatrix} =R_{b}^{c}\begin{Bmatrix} R_{w}^{b_{j}}[R_{b_{i}}^{w}(R^{b}_{c} \frac{1}{\lambda} \begin{bmatrix} u_{c_{i}} \\ v_{c_{i}} \\ 1 \end{bmatrix} + p_{c}^{b}) + p_{b_{i}}^{w}]-(R_{b_{j}}^{w})^{T} p_{b_{j}}^{w}\end{Bmatrix} - (R_{c}^{b})^{T}p_{c}^{b} \\ =R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} \frac{1}{\lambda} \begin{bmatrix} u_{c_{i}} \\ v_{c_{i}} \\ 1\end{bmatrix} + R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})$

而觀測值即$P$在第$j$幀被觀測到的歸一化相機座標系下的座標爲$\begin{bmatrix} u_{c_{j}} \\ v_{c_{j}} \\ 1 \end{bmatrix}$，因而得到視覺殘差爲：$r_{c} = \begin{bmatrix} \frac{x_{c_{j}}}{z_{c_{j}}} - u_{c_{j}} \\ \frac{y_{c_{j}}}{z_{c_{j}}} - v_{c_{j}}\end{bmatrix}$

---

以上是對針孔模型的視覺殘差，而在VINS論文中其實顯示的是對一般相機模型的視覺殘差，即使用了廣角相機的球面模型。
最後定義的視覺殘差爲：【論文公式(25)】$r_{\mathcal{C}}(\hat{z_{l}}^{c_{j}}, \mathcal{X}) = \begin{bmatrix} \overrightarrow{b_{1}} \\ \overrightarrow{b_{2}} \end{bmatrix} (\frac{P_{l}^{c_{j}}}{\left \| P_{l}^{c_{j}} \right \|}-\bar{P_{l}}^{c_{j}})$ 其中，$\bar{P}_{l}^{c_{j}}$爲第$l$個路標點在第$j$個相機歸一化相機座標系中的觀察到的座標爲：$\bar P_{l}^{c_{j}} = \pi_{c}^{-1}(\begin{bmatrix} \hat{u}_{l}^{c_{j}}\\ \hat{v_{l}^{c_{j}}}\end{bmatrix})$ 另外，$P_{l}^{c_{j}}$是估計第$l$個路標點在第$j$個相機歸一化相機座標系中的可能座標，推導如下：$P_{l}^{c_{j}} = R_{b}^{c}\begin{Bmatrix}R_{w}^{b_{j}}[R_{b_{i}}^{w}(R_{c}^{b} \frac{1}{\lambda} \bar{P}_{l}^{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w}]-p_{c}^{b}\end{Bmatrix}$ 因爲視覺殘差的自由度爲2, 因此將視覺殘差投影到正切平面上，$b_{1}, b_{2}$爲正切平面上的任意倆個正交基。

---

在代碼中，針孔模型和廣角相機的球面模型都有被用到：

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

5.2 視覺優化變量

第$l$個路標點在$i$和$j$時刻構建的殘差，優化變量包括倆個時刻的狀態量、外參、以及路標點的逆深度(路標點第一次被觀察到的時候確定的值)：$[p_{b_{i}}^{w}, q_{b_{i}}^{w}], \ [p_{b_{j}}^{w}, q_{b_{j}}^{w}], \ [p_{c}^{b}, q_{c}^{b}], \ \lambda_{l}$

5.3 視覺重投影誤差對狀態量的雅克比矩陣

這裏採用針孔相機的(歸一化相機平面)視覺殘差來進行推導，而相機球面模型的視覺殘差推導過程也相似。$r_{c} = \begin{bmatrix} \frac{x_{c_{j}}}{z_{c_{j}}} - u_{c_{j}} \\ \frac{y_{c_{j}}}{z_{c_{j}}} - v_{c_{j}}\end{bmatrix}$ 這裏先定義：$f_{c_{i}} = \frac{1}{\lambda_{l}} \bar P_{l}^{c_{i}} = \frac{1}{\lambda} \begin{bmatrix} u_{c_{i}} \\ v_{c_{i}} \\ 1\end{bmatrix}$ $f_{c_{j}}$即原來的$\mathcal{P}_{l}^{c_{j}}:$ $f_{c_{j}}=R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} f_{c_{i}} + R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})$ 即 $f_{c_{j}}= R_{b}^{c}\begin{Bmatrix}R_{w}^{b_{j}} \begin{bmatrix}R_{b_{i}}^{w}(R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} -p_{c}^{b}\end{Bmatrix}$
根據鏈式法則，對殘差對每個雅克比的計算可以分成：
1）視覺殘差對重投影3D點$f_{c_{j}}$求導：$\frac{\partial r_{c}}{\partial f_{c_{j}}} = \begin{bmatrix} \frac{1}{z_{c_{j}}} & 0 & -\frac{x_{c_{j}}}{z_{c_{j}}^{2}} \\ 0 & \frac{1}{z_{c_{j}}} & -\frac{y_{c_{j}}}{z_{c_{j}}^{2}}\end{bmatrix}$
2) $f_{c_{j}}$ 對各個優化變量的求導：$J[0]^{3 \times 7} = [\frac{\partial{f_{c_{j}}}}{\partial{p_{b_{i}}^{w}}}, \frac{\partial{f_{c_{j}}}}{\partial{q_{b_{i}}^{w}}}] = \begin{bmatrix} R_{b}^{c}R_{w}^{b_{j}}, & -R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w} [R_{c}^{b}\frac{1}{\lambda_{l}}\bar{P}_{l}^{c_{i}}+p_{c}^{b}]_{\times}\end{bmatrix} \tag{11}$ $J[1]^{3 \times 7} = [\frac{\partial{f_{c_{j}}}}{\partial{p_{b_{j}}^{w}}}, \frac{\partial{f_{c_{j}}}}{\partial{q_{b_{j}}^{w}}}] = \begin{bmatrix} -R_{b}^{c}R_{w}^{b_{j}}, & R_{b}^{c}\begin{Bmatrix}R_{w}^{b_{j}}[R_{b_{i}}^{w} (\frac{1}{\lambda_{l}}R_{c}^{b}\bar{P}_{l}^{c_{i}}+p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w}]\end{Bmatrix}_{\times}\end{bmatrix} \tag{12}$ $J[2]^{3 \times 7} = [\frac{\partial{f_{c_{j}}}}{\partial{p_{c}^{b}}}, \frac{\partial{f_{c_{j}}}}{\partial{q_{c}^{b}}}] = \begin{bmatrix} R_{b}^{c}(R_{w}^{b_{j}}R_{b_{i}}^{w}-I_{3\times 3}) \\ -R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} [\frac{1}{\lambda_{l}}\bar{P}_{l}^{c_{i}}]_{\times}+[R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} \frac{1}{\lambda_{l}}\bar{P}_{l}^{c_{i}}]_{\times}+ \begin{Bmatrix}R_{b}^{c} [R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b}]\end{Bmatrix}_{\times}\end{bmatrix} \tag{13}$ $J[3]^{3\times 1} = \frac{\partial f_{c_{j}}}{\lambda _{l}} = -R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}\frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}^{2}} \tag{14}$

這一部分的具體代碼實現在projection_factor.cpp的bool ProjectionFactor::Evaluate()函數中。

---

以下是對公式$(11)(12)(13)(14)$的推導：
由上面公式得：$f_{c_{j}}= R_{b}^{c}\begin{Bmatrix}R_{w}^{b_{j}} \begin{bmatrix}R_{b_{i}}^{w}(R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} -p_{c}^{b}\end{Bmatrix}$

• 對$i$時刻位移$p_{b_{i}}^{w}$的求導：$\frac{\partial{f_{c_{j}}}}{\partial{p_{b_{i}}^{w}}} = R_{b}^{c}R_{w}^{b_{j}}$
• 對$i$時刻旋轉$q_{b_{i}}^{w}$的求導：$\frac{\partial{f_{c_{j}}}}{\partial{q_{b_{i}}^{w}}} = \frac{\partial{f_{c_{j}}}}{\partial{\delta \theta_{b_{i}}}} = \frac{R_{b}^{c}\begin{Bmatrix}R_{w}^{b_{j}} \begin{bmatrix}R_{b_{i}}^{w} exp([\delta \theta_{b_{i}}]_{\times})(R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} -p_{c}^{b}\end{Bmatrix}}{\partial{\delta \theta_{b_{i}}}} \\ = \frac{R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} exp([\delta \theta_{b_{i}}]_{\times})(R_{c}^{b} f_{c_{i}} + p_{c}^{b})}{\partial{\delta \theta_{b_{i}}}} \\ = - \frac{R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} [R_{c}^{b} f_{c_{i}} + p_{c}^{b}]_{\times} \delta \theta_{b_{i}}}{\partial{\delta \theta_{b_{i}}}} \\ =-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} [R_{c}^{b} f_{c_{i}} + p_{c}^{b}]_{\times}$
• 對$j$時刻位姿$p_{b_{j}}^{w}$的求導：$\frac{\partial{f_{c_{j}}}}{\partial{p_{b_{i}}^{w}}} = -R_{b}^{c}R_{w}^{b_{j}}$
• 對$j$時刻旋轉$q_{b_{j}}^{w}$的求導：$\frac{\partial{f_{c_{j}}}}{\partial{q_{b_{j}}^{w}}} = \frac{\partial{f_{c_{j}}}}{\partial{\delta \theta_{b_{i}}}} = \frac{\partial R_{b}^{c}\begin{Bmatrix}(R^{w}_{b_{j}} exp([\delta \theta_{b_{i}}]_{\times}))^{-1} \begin{bmatrix}R_{b_{i}}^{w} (R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} -p_{c}^{b}\end{Bmatrix}}{\partial{\delta \theta_{b_{i}}}} \\ = \frac{\partial R_{b}^{c} exp(-[\delta \theta_{b_{i}}]_{\times}) R_{w}^{b_{j}}\begin{bmatrix}R_{b_{i}}^{w} (R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} }{\partial{\delta \theta_{b_{i}}}} \\ = \frac{\partial R_{b}^{c} \begin{Bmatrix} R_{w}^{b_{j}}\begin{bmatrix}R_{b_{i}}^{w} (R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} \end{Bmatrix}_{\times} \delta \theta_{b_{i}}}{\partial{\delta \theta_{b_{i}}}} \\ = R_{b}^{c} \begin{Bmatrix} R_{w}^{b_{j}}\begin{bmatrix}R_{b_{i}}^{w} (R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} \end{Bmatrix}_{\times}$
• 對外參的旋轉$q_{c}^{b}$的求導：$f_{c_{j}}= R_{b}^{c}\begin{Bmatrix}R_{w}^{b_{j}} \begin{bmatrix}R_{b_{i}}^{w}(R_{c}^{b} f_{c_{i}} + p_{c}^{b})+p_{b_{i}}^{w}-p_{b_{j}}^{w} \end{bmatrix} -p_{c}^{b}\end{Bmatrix} \\ = R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} f_{c_{i}} + R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b}) = f_{c_{j}}^{1} + f_{c_{j}}^{2}$ 分成倆項分別求導，其中$o^{2}(\delta \theta_{c}^{b})$代表關於$\delta \theta_{c}^{b}$的二階項：$\frac{\partial{f_{c_{j}}^{1}}}{\partial{q_{c}^{b}}} = \frac{\partial{f_{c_{j}}^{1}}}{\partial \delta \theta_{c}^{b}} = \frac{\partial (R_{c}^{b} exp([\delta \theta_{c}^{b}]_{\times}))^{-1}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} exp([\delta \theta_{c}^{b}]_{\times})f_{c_{i}}}{\partial \delta \theta_{c}^{b}} \\ = \frac{\partial exp(-[\delta \theta_{c}^{b}]_{\times})R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b} exp([\delta \theta_{c}^{b}]_{\times})f_{c_{i}}}{\partial \delta \theta_{c}^{b}} \\ \approx \frac{\partial (I-[\delta \theta_{c}^{b}]_{\times})R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}(I+[\delta \theta_{c}^{b}]_{\times}) f_{c_{i}}}{\partial \delta \theta_{c}^{b}} \\ = \frac{\partial (-[\delta \theta_{c}^{b}]_{\times})R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}f_{c_{i}} + R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}([\delta \theta_{c}^{b}]_{\times}) f_{c_{i}} + o^{2}(\delta \theta_{c}^{b})}{\partial \delta \theta_{c}^{b}} \\ = \frac{\partial [R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}f_{c_{i}}]_{\times} \delta \theta_{c}^{b} - R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}[f_{c_{i}}]_{\times} \delta \theta_{c}^{b}+ o^{2}(\delta \theta_{c}^{b})}{\partial \delta \theta_{c}^{b}} \\ = [R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}f_{c_{i}}]_{\times} - R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}[f_{c_{i}}]_{\times}$ $\frac{\partial f_{c_{j}}^{2}}{\partial q_{c}^{b}} = \frac{\partial f_{c_{j}}^{2}}{\partial \delta \theta_{c}^{b}} = \frac{\partial (R_{c}^{b} exp([\delta \theta_{c}^{b}]_{\times}))^{-1}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})}{\partial \delta \theta_{c}^{b}} \\ = \frac{\partial exp(-[\delta \theta_{c}^{b}]_{\times}) R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})}{\partial \delta \theta_{c}^{b}} \\ \approx \frac{\partial (-[\delta \theta_{c}^{b}]_{\times}) R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})}{\partial \delta \theta_{c}^{b}} \\= \frac{\partial [R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})]_{\times} \delta \theta_{c}^{b}}{\partial \delta \theta_{c}^{b}} \\ = [R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})]_{\times}$ 故 $\frac{\partial{f_{c_{j}}}}{\partial{q_{c}^{b}}} = \frac{\partial{f_{c_{j}}^{1}}}{\partial{q_{c}^{b}}} + \frac{\partial{f_{c_{j}}^{2}}}{\partial{q_{c}^{b}}} = [R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}f_{c_{i}}]_{\times} - R_{b}^{c} R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}[f_{c_{i}}]_{\times}+[R_{b}^{c}(R_{w}^{b_{j}}(R_{b_{i}}^{w} p_{c}^{b}+ p_{b_{i}}^{w}-p_{b_{j}}^{w})-p_{c}^{b})]_{\times}$
• 對逆深度$\lambda$的求導: $\frac{\partial f_{c_{j}}}{\partial \lambda} = \frac{\partial f_{c_{j}}}{\partial f_{c_{i}}} \frac{\partial f_{c_{i}}}{\partial \lambda} = -R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}\frac{1}{\lambda ^{2}}\begin{bmatrix} u_{c_{i}} \\ v_{c_{i}} \\ 1 \end{bmatrix} = -R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}\frac{1}{\lambda ^{2}} \bar{P}^{c_{i}}$

至此相機重投影誤差對各個狀態量的雅克比矩陣已經推導完畢，推導過程中規中矩，需要對哪個變量進行求導，則對其添加擾動。

---

4、協方差

視覺約束的協方差與標定相機內參時的重投影誤差有關。VINS在代碼中sqrt_info取1.5個像素，對應到歸一化相機平面還需除以焦距f，最後得到的信息矩陣：（這裏真正的信息矩陣其實是sqrt_info的平方）
$sqrt(\Omega_{vis}) = sqrt(\Sigma^{-1}_{vis}) = (1.5/f \ I_{2 \times 2})^{-1} = f/1.5 \ I_{2\times2}$

void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

總結：

1. 相機構建的殘差的特徵點逆深度在第$i$幀中初始化得到，在第$j$幀被觀察到，因此 $i$和$j$不一定是相鄰倆幀，而IMU的增量誤差一般爲相鄰時刻$k$和$k+1$的誤差。
2. 對於IMU預積分增量誤差或者視覺重投影誤差對優化量的偏導，如果對位姿（四元數）求導，則用擾動模型進行推導，如果是其他狀態量，就直接對狀態量求偏導。
3. IMU約束的協方差是從上一時刻通過遞推方程傳遞到當前時刻的，而視覺的協方差是獨立的，由當前時刻確定的常量。