VINS-Mono理論推導2

Reference

1. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.
2. Quaternion Kinematics for Error-State KF.

 

---

視覺部分的優化問題建模

視覺部分的問題就是一個BA問題，不過與純視覺不同，這裏涉及到兩個座標系之間的變換，分別是camera座標系C和IMU座標系（通常認爲這個是機體座標系）b，同時因爲整個過程中使用的是逆深度表示空間中的Landmark，因此這個投影過程會變得稍微麻煩一些，但是整個過程還是比較清晰的。
$\begin{aligned} \begin{cases} P_{ci} &= \frac{f_{ci}}{\rho} \\ P_{bi} &= R^{b}_{c}P_{ci}+t^{b}_{c} \\ P_w &= R^{w}_{bi}P_{bi}+t^{w}_{bi} \\ P_{bj} &= (R^{w}_{bj})^T(P_w-t^{w}_{bj}) \\ P_{cj} &= (R^{b}_{c})^T(P_{bj}-t^{b}_{c}) = \left[ X_j, Y_j, Z_j \right]^T \\ p_{cj} &= \left[ \frac{X_j}{Z_j}, \frac{Y_j}{Z_j} \right]^T \\ error_{ij} &= \left[ \frac{X_j}{Z_j}-u_j, \frac{Y_j}{Z_j}-v_j \right]^T \end{cases} \end{aligned} \tag{1}$
於是可以逐個求解Jacobian矩陣了：

對第 i 幀的位姿進行求導

$\begin{aligned} \frac{\partial{e_{ij}}}{\partial{T^{w}_{bi}}} &= \left[\frac{\partial{e_{ij}}}{\partial{t^{w}_{bi}}}, \frac{\partial{e_{ij}}}{\partial{R^{w}_{bi}}}\right] \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{P_{bj}}} \frac{\partial{P_{bj}}}{\partial{P_{w}}} \left[\frac{\partial{P_w}}{\partial{t^{w}_{bi}}}, \frac{\partial{P_w}}{\partial{R^{w}_{bi}}}\right] \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3} (R^{b}_{c})^T_{3\times3}(R^{w}_{bj})^T_{3\times3}\left[I, -R^{w}_{bi}[P_{bi}]_{\times} \right]_{3\times6} \end{aligned} \tag{2}$

對第 j 幀的位姿進行求導

$\begin{aligned} \frac{\partial{e_{ij}}}{\partial{T^{w}_{bj}}} &= \left[\frac{\partial{e_{ij}}}{\partial{t^{w}_{bj}}}, \frac{\partial{e_{ij}}}{\partial{R^{w}_{bj}}}\right] \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{P_{bj}}} \left[\frac{\partial{P_{bj}}}{\partial{t^{w}_{bj}}}, \frac{\partial{P_{bj}}}{\partial{R^{w}_{bj}}}\right] \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3} (R^{b}_{c})^T_{3\times3}\left[-(R^{w}_{bj})^T, [P_{bj}]_{\times} \right]_{3\times6} \end{aligned} \tag{3}$

對逆深度$\rho$進行求導

$\begin{aligned} \frac{\partial{e_{ij}}}{\partial{\rho}} &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{P_{bj}}}\frac{\partial{P_{bj}}}{\partial{P_{w}}}\frac{\partial{P_{w}}}{\partial{P_{bi}}}\frac{\partial{P_{bi}}}{\partial{P_{ci}}}\frac{\partial{P_{ci}}}{\partial{\rho}} \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3} (R^{b}_{c})^T_{3\times3}(R^{w}_{bj})^T_{3\times3}(R^{w}_{bi})_{3\times3}(R^{b}_{c})_{3\times3}(-\frac{f_i}{\rho^2})_{3\times1} \end{aligned} \tag{4}$

對IMU和Camera的外參進行求導

$\begin{aligned} \frac{\partial{e_{ij}}}{\partial{T^{b}_{c}}} &= \left[\frac{\partial{e_{ij}}}{\partial{t^{b}_{c}}}, \frac{\partial{e_{ij}}}{\partial{R^{b}_{c}}}\right] \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{T^{b}_{c}}} \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}(\frac{\partial{P_{cj}}}{\partial{T^{b}_{c}}}+(R^{b}_{c})^T\frac{\partial{P_{bj}}}{\partial{T^{b}_{c}}}) \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}(\frac{\partial{P_{cj}}}{\partial{T^{b}_{c}}}+(R^{b}_{c})^T\frac{\partial{P_{bj}}}{\partial{P_{w}}}\frac{\partial{P_{w}}}{\partial{P_{bi}}}\frac{\partial{P_{bi}}}{\partial{T^{b}_{c}}}) \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3}\left( \left[ -(R^{b}_{c})^T, [P_{cj}]_{\times} \right] + (R^{b}_{c})^T(R^{w}_{bj})^TR^{w}_{bi} \left[ I, -R^{b}_{c}[P_{ci}]_{\times} \right] \right)_{3\times6} \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3}\left( \left[ (R^{b}_{c})^T((R^{w}_{bj})^TR^{w}_{bi}-I), ([P_{cj}]_{\times}-(R^{b}_{c})^T(R^{w}_{bj})^TR^{w}_{bi}R^{b}_{c}[P_{ci}]_{\times}) \right] \right)_{3\times6} \end{aligned} \tag{5}$

在VINS-Mono的代碼中，作者把$[P_{ci}]_{\times}$給分解了，這塊兒不太理解作者爲什麼要分解開，但是其實展開之後的結果是一樣的。

 

---

慣導部分的優化問題建模

慣導部分的建模也是一個最小化誤差的問題，該誤差部分由兩個部分組成，一部分是狀態變量組成的幀間間隔，這部分與位置、角度、速度相關，與bias不相關：
$\begin{aligned} \begin{cases} p^{bi}_{bj} &= (R^{w}_{bi})^T \left[({^w}p_{bj}-{^{w}}p_{bi})-{^w}v_{bi}\Delta{t}+\frac{1}{2}{^{w}}g\Delta{t}^2\right] \\ v^{bi}_{bj} &= (R^{w}_{bi})^T\left[({^w}v_{bj} - {^w}v_{bi})+{^{w}}g\Delta{t}\right] \\ q^{bi}_{bj} &= (q^{w}_{bi})^{-1}\otimes(q^{w}_{bj}) \\ ba_{j} &= ba_{i} \\ bg_{j} &= bg_{i} \end{cases} \end{aligned} \tag{6}$
另一部分是IMU的積分值$\{\overline{p}^{bi}_{bj}, \overline{v}^{bi}_{bj}, \overline{q}^{bi}_{bj}\}$，這部分因爲是在$bi$系下的積分，因此與位置、角度、速度（均在世界座標系下）無關，但是這部分與bias息息相關，所以這部分筆者認爲不能叫觀測值；

所以整個問題的誤差爲：
$\begin{aligned} \begin{cases} e_p &= p^{bi}_{bj}-\overline{p}^{bi}_{bj}=(R^{w}_{bi})^T \left[({^w}p_{bj}-{^{w}}p_{bi})-{^w}v_{bi}\Delta{t}+\frac{1}{2}{^{w}}g\Delta{t}^2\right]-\overline{p}^{bi}_{bj} = (R^{w}_{bi})^T {^w}\Delta{p} - \overline{p}^{bi}_{bj} \\ e_q &= 2[ (\overline{q}^{bi}_{bj})^{-1} \otimes q^{bi}_{bj}]_{3\times1} \\ e_v &= v^{bi}_{bj}-\overline{v}^{bi}_{bj}=(R^{w}_{bi})^T({^w}v_{bj} - {^w}v_{bi}+{^{w}}g\Delta{t})-\overline{v}^{bi}_{bj} = (R^{w}_{bi})^T {^w}\Delta{v}-\overline{v}^{bi}_{bj} \\ e_{ba} &= ba_j - ba_i \\ e_{bg} &= bg_j - bg_i \end{cases} \end{aligned} \tag{7}$
我這裏稍微有一個問題就是在代碼中的姿態誤差$e_q$和論文中的公式並不相同，起初筆者還是比較在意這個事情的，但是後來個人認爲只要整個推導過程中按照一個公式來就可以了。

所以根據公式（7）對狀態變量進行求導（按照VINS-Mono中的組織形式）：

誤差部分對 i 時刻狀態量的求導

誤差部分對於i 時刻位姿狀態量的求導

$\begin{aligned} \frac{\partial{e}}{\partial{T_{bi}}} &= \left[\frac{\partial{e}}{\partial{p_{bi}}}, \frac{\partial{e}}{\partial{\theta_{bi}}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{p_{bi}}} & \frac{\partial{e_p}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_q}}{\partial{p_{bi}}} & \frac{\partial{e_q}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_v}}{\partial{p_{bi}}} & \frac{\partial{e_v}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_{ba}}}{\partial{p_{bi}}} & \frac{\partial{e_{ba}}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_{bg}}}{\partial{p_{bi}}} & \frac{\partial{e_{bg}}}{\partial{\theta_{bi}}} \\ \end{bmatrix}\\ &= \begin{bmatrix} -(R^{w}_{bi})^T & [(R^{w}_{bi})^T \Delta{p}]_{\times} \\ 0 & -\left[ [q^{j}_{i}]_{\mathrm{L}} [\overline{q}^{i}_{j} \otimes \mathrm{J}^{q}_{bg}\delta{bg_{i}}]_{\mathrm{R}} \right]_{3\times3} \\ 0 & [(R^{w}_{bi})^T {^w}\Delta{v}]_{\times} \\ 0 & 0 \\ 0 & 0 \end{bmatrix}\\ \end{aligned} \tag{8}$

這裏簡單推導一下姿態誤差對姿態的求導：
$\begin{aligned} e(\theta+\delta{\theta}) &= 2[(\overline{q}^{i}_{j})^{-1} \otimes [(q^{w}_{i}\otimes q(\delta{\theta}))^{-1}\otimes q^{w}_{j}]]_{3\times1} \\ &= 2[(\overline{q}^{i}_{j})^{-1} \otimes q(\delta{\theta})^{-1}\otimes q^{i}_{j}]_{3\times1} \\ &= 2[[(q^{i}_{j})^{-1}\otimes q(\delta{\theta}) \otimes \overline{q}^{i}_{j})]^{-1}]_{3\times1} \\ &= 2[[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix}]^{-1}]_{3\times1} \\ &= 2[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix}1 \\ -\frac{1}{2}\delta{\theta}\end{bmatrix}]_{3\times1} \\ &= 2[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix}1 \\ 0\end{bmatrix}+[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix} 0 \\ -\frac{1}{2}\delta{\theta} \end{bmatrix}]_{3\times1} \\ &= e(\theta)+[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix} 0 \\ -\delta{\theta} \end{bmatrix}]_{3\times1} \end{aligned}$
這部分其實看到中間取了一個求逆操作，主要是爲了和VINS-Mono中的公式對上。

所以採用求導的原始定義有：
$\frac{\partial{e_q}}{\partial{\theta}}=\mathrm{lim}\frac{e(\theta+\delta{\theta})-e(\theta)}{\delta{\theta}}=-[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}]_{3\times3}$

誤差部分對於 i 時刻運動狀態量的求導

$\begin{aligned} \frac{\partial{e}}{\partial{M_{bi}}} &= \left[\frac{\partial{e}}{\partial{v_{bi}}}, \frac{\partial{e}}{\partial{ba_i}}, \frac{\partial{e}}{\partial{bg_i}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{v_{bi}}} & \frac{\partial{e_p}}{\partial{ba_i}} & \frac{\partial{e_p}}{\partial{bg_i}} \\ \frac{\partial{e_q}}{\partial{v_{bi}}} & \frac{\partial{e_q}}{\partial{ba_i}} & \frac{\partial{e_q}}{\partial{bg_i}} \\ \frac{\partial{e_v}}{\partial{v_{bi}}} & \frac{\partial{e_v}}{\partial{ba_i}} & \frac{\partial{e_v}}{\partial{bg_i}} \\ \frac{\partial{e_{ba}}}{\partial{v_{bi}}} & \frac{\partial{e_{ba}}}{\partial{ba_i}} & \frac{\partial{e_{ba}}}{\partial{bg_i}} \\ \frac{\partial{e_{bg}}}{\partial{v_{bi}}} & \frac{\partial{e_{bg}}}{\partial{ba_i}} & \frac{\partial{e_{bg}}}{\partial{bg_i}} \\ \end{bmatrix}\\ &= \begin{bmatrix} -(R^{w}_{bi})^T \Delta{t} & -J^{p}_{ba_i} & -J^{p}_{bg_i} \\ 0 & -J^{q}_{ba_i} & -[[q^{j}_{i}\otimes \overline{q}^{i}_{j}]_{\mathrm{L}}]_{3\times3}J^{q}_{bg_i} \\ -(R^{w}_{bi})^T & -J^{v}_{ba_i} & -J^{v}_{bg_i} \\ 0 & -I & 0 \\ 0 & 0 &-I \end{bmatrix}\\ \end{aligned} \tag{9}$

依舊簡單推導一下姿態誤差對角速度bias的求導過程：
$\begin{aligned} e(bg+\delta{bg}) &= 2[(\overline{q}^{i}_{j} \otimes \begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix})^{-1} \otimes q^{i}_{j}]_{3\times1} \\ &= 2[\begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix}^{-1}(\overline{q}^{i}_{j})^{-1} \otimes q^{i}_{j}]_{3\times1} \\ &= 2[[(q^{i}_{j})^{-1}\otimes \overline{q}^{i}_{j}) \otimes \begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix} ]^{-1}]_{3\times1} \\ &= 2[[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix} ]^{-1}]_{3\times1} \\ &= 2[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}1 \\ -\frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix}]_{3\times1} \\ &= 2[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}1 \\ 0 \end{bmatrix}]_{3\times1} + 2[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}0 \\ -\frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix}]_{3\times1} \\ &= e(bg) + [[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}0 \\ -J^{q}_{bg} \delta{bg}\end{bmatrix}]_{3\times1} \end{aligned}$
這部分很重要的一定是通過四元數的求逆法則將$\delta{bg}$轉移到公式的最後，最後再通過四元數求逆其實就是把四元數的虛部部分添加負號的方法把求逆符號由公式外轉到宮室內。

同樣使用求導的定義有：
$\frac{\partial{e_q}}{\partial{bg}}=\mathrm{lim}\frac{e(bg+\delta{bg})-e(bg)}{\delta{bg}}=-[[q^{j}_{i}\otimes \overline{q}^{i}_{j}]_{\mathrm{L}}]_{3\times3}J^{q}_{bg}$

誤差部分對 j 時刻狀態量的求導

誤差部分對於 j 時刻位姿狀態量的求導

$\begin{aligned} \frac{\partial{e}}{\partial{T_{bj}}} &= \left[\frac{\partial{e}}{\partial{p_{bj}}}, \frac{\partial{e}}{\partial{\theta_{bj}}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{p_{bj}}} & \frac{\partial{e_p}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_q}}{\partial{p_{bj}}} & \frac{\partial{e_q}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_v}}{\partial{p_{bj}}} & \frac{\partial{e_v}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_{ba}}}{\partial{p_{bj}}} & \frac{\partial{e_{ba}}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_{bg}}}{\partial{p_{bj}}} & \frac{\partial{e_{bg}}}{\partial{\theta_{bj}}} \\ \end{bmatrix}\\ &= \begin{bmatrix} (R^{w}_{bi})^T & 0 \\ 0 & \left[ \left[(\overline{q}^{i}_{j} \otimes \mathrm{J}^{q}_{bg}\delta{bg_{i}})^{-1} \otimes q^{i}_{j} \right]_{\mathrm{R}} \right]_{3\times3} \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \end{bmatrix}\\ \end{aligned} \tag{10}$

簡單推導一下姿態誤差對姿態的求導：
$\begin{aligned} e(\theta+\delta{\theta}) &= 2[(\overline{q}^{i}_{j})^{-1} \otimes (q^{w}_{i})^{-1}\otimes q^{w}_{j}\otimes q(\delta{\theta})]_{3\times1} \\ &= 2[(\overline{q}^{i}_{j})^{-1}\otimes q^{i}_{j} \otimes \begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix}]_{3\times1} \\ &= 2[[(\overline{q}^{i}_{j})^{-1}\otimes q^{i}_{j}]_{\mathrm{L}} \begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix}]_{3\times1} \\ &= e(\theta) + [[(\overline{q}^{i}_{j})^{-1}\otimes q^{i}_{j}]_{\mathrm{L}} \begin{bmatrix}0 \\ \delta{\theta}\end{bmatrix}]_{3\times1} \end{aligned}$

誤差部分對於 j 時刻運動狀態量的求導

$\begin{aligned} \frac{\partial{e}}{\partial{M_{bj}}} &= \left[\frac{\partial{e}}{\partial{v_{bj}}}, \frac{\partial{e}}{\partial{ba_j}}, \frac{\partial{e}}{\partial{bg_j}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{v_{bj}}} & \frac{\partial{e_p}}{\partial{ba_j}} & \frac{\partial{e_p}}{\partial{bg_j}} \\ \frac{\partial{e_q}}{\partial{v_{bj}}} & \frac{\partial{e_q}}{\partial{ba_j}} & \frac{\partial{e_q}}{\partial{bg_j}} \\ \frac{\partial{e_v}}{\partial{v_{bj}}} & \frac{\partial{e_v}}{\partial{ba_j}} & \frac{\partial{e_v}}{\partial{bg_j}} \\ \frac{\partial{e_{ba}}}{\partial{v_{bj}}} & \frac{\partial{e_{ba}}}{\partial{ba_j}} & \frac{\partial{e_{ba}}}{\partial{bg_j}} \\ \frac{\partial{e_{bg}}}{\partial{v_{bj}}} & \frac{\partial{e_{bg}}}{\partial{ba_j}} & \frac{\partial{e_{bg}}}{\partial{bg_j}} \\ \end{bmatrix}\\ &= \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ (R^{w}_{bi})^T & 0 & 0 \\ 0 & I & 0 \\ 0 & 0 & I \end{bmatrix}\\ \end{aligned} \tag{11}$

小結一下慣性部分的建模問題

1. 這部分的誤差模型其實由兩個部分組成，一部分是狀態量構建的幀間變化量（與位姿和速度直接相關），另一部分是IMU預積分得到的幀間變化量，這部分由於是以 i 時刻作爲參考系，因此和位姿速度無直接關係，但是在預積分過程中和角速度計和角速度計的bias息息相關；
2. 在預積分過程中，雖然使用了精度較高的中點法，但是整個預積分過程中並沒有涉及到 j 時刻的零偏，或者說認爲 j 時刻的零偏與 i 時刻是一樣的，所以看到誤差部分對 j 時刻狀態的求導是比較簡單的；

 

---

小結

建模這部分就先到這裏，下部分關注的重點就是邊緣化和優化問題的先驗部分了。