接下來 範圍 del spa jacob 處的 HR 每次 過大

給 StereoDSO 加 IMU，想直接用 OKVIS 的代碼，但是有點看不懂。知乎上鄭帆寫的文章《四元數矩陣與 so(3) 左右雅可比
》提到 OKVIS 的預積分是使用四元數，而預積分論文中使用 so(3) 的右雅克比。才疏學淺，先整理好 so(3) 的預積分，寫好 StereoDSO 加上 IMU，再考慮其他的東西。
以下的內容參考預積分的的論文，還有它的 Supplementary Material。預積分的論文中有一些 typo 所以看上去還是比較迷的，參考網絡上多份預積分論文的 pdf，我整理一下整個預積分的推導過程。所以以下內容也可以看做是預積分內容的翻譯。

預積分的意義。IMU 積分是在 Manifold 上積分，在 IMU 測量值一定的情況下，由於積分起點的不同，積分的終點也不同。IMU 測量值可以組成一個觀測方程，通過觀測方程與當前起點與終點狀態量的估計值可以計算出 IMU 觀測方程的誤差。將 IMU 觀測方程形成的誤差與視覺部分誤差一同優化，得到最終起點與終點狀態量的估計值。因為這是一個非線性優化，通過叠代實現，每次叠代都需要進行 IMU 積分。相對於每次叠代的其他部分計算 IMU 積分耗時長。預積分的作用深入考慮 IMU 積分過程，將大量的 IMU 測量值整合成有限個虛擬測量值，並且在一定範圍內保證虛擬測量值與起點的狀態值無關，以此減少 IMU 積分的次數。

Residuals

IMU 測量值可以看做真實值、漂移值（Bias）、高斯誤差的和：
\[\begin{align} \mathbin{_B\tilde{\pmb{\omega}}_{WB}}(t) = \mathbin{_B\pmb{\omega}_{WB}}(t) + \pmb{b}^g(t) + \pmb{\eta}^g(t) \label{eq2:imu_pre_mes_w} \\mathbin{_B\tilde{\pmb{a}}}(t) = \mathbf{R}_{WB}^T(t)(\mathbin{_W\pmb{a}}(t) - \mathbin{_W\pmb{g}}) + \pmb{b}^a(t) + \pmb{\eta}^a(t) \label{eq2:imu_pre_mes_a} \end{align}\]

其中下標的 \(W, B\) 分別表示世界（World）坐標系和 IMU （Body）坐標系，左下標 \(\mathbin{_W\cdot}, \mathbin{_B\cdot}\) 表示所在的坐標系，\(\mathbf{R}_{WB}\) 的右下標表示從 IMU 坐標系旋轉到世界坐標系，\(\mathbin{_B\pmb{\omega}_{WB}}\) 的右下標表示 IMU 坐標系相對於世界坐標系的瞬時角速度，右上標 \(\cdot^{g}, \cdot^{a}\) 分別表示陀螺儀（Gyroscope）角速度測量值相關、加速度計（Accelerometer）線加速度測量值相關。公式中的真實值是 \(t\) 時刻的角速度 \(\mathbin{_B\pmb{\omega}_{WB}}(t)\)、線加速度 \(\mathbin{_W\pmb{a}}(t)\)，漂移值是 \(t\) 時刻的角速度漂移值 \(\pmb{b}^g(t)\)、線加速度漂移值 \(\pmb{b}^a(t)\)，高斯誤差是 \(t\) 時刻的角速度測量值誤差 \(\pmb{\eta}^g(t)\)、線加速度測量誤差 \(\pmb{\eta}^a(t)\)。\(\mathbin{_W\pmb{g}}\) 是世界坐標系下的重力加速度。

從 \(t\) 時刻開始，經過一小段時間 \(\Delta t\) 到達 \(t + \Delta t\) 時刻，使用歐拉方法（Euler Method），即假設在 $[t, t + \Delta t] $ 內加速度與線加速度保持不變，得到以下結果：
\[\begin{align} \mathbf{R}_{WB}(t+\Delta t) &= \mathbf{R}_{WB}(t) \exp(\mathbin{_B\pmb{\omega}_{WB}}(t)\Delta t) \ \mathbin{_W\mathbf{v}}(t+\Delta t) &= \mathbin{_W\mathbf{v}}(t) + \mathbin{_W\mathbf{a}}(t)\Delta t \ \mathbin{_W\mathbf{p}}(t+\Delta t) &= \mathbin{_W\mathbf{p}}(t) + \mathbin{_W\mathbf{v}}(t) \Delta t + \frac{1}{2}\mathbin{_W\mathbf{a}}(t)\Delta t^2 \end{align}\]
將公式 (\ref{eq2:imu_pre_mes_w}) (\ref{eq2:imu_pre_mes_a})，並忽略表示坐標系的下標，得到：
\[\begin{align} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t)\exp((\tilde{\pmb{\omega}}(t) - \mathbf{b}^g(t) - \pmb{\eta}^{gd}(t))\Delta t) \label{eq2:imu_pre_delta_R} \ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g}\Delta t + \mathbf{R}(t)(\tilde{\mathbf{a}}(t) - \mathbf{b}^a(t) - \pmb{\eta}^{ad}(t))\Delta t \label{eq2:imu_pre_delta_v} \ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t)\Delta t + \frac{1}{2}\mathbf{g}\Delta t^2 + \frac{1}{2}\mathbf{R}(t)(\tilde{\mathbf{a}}(t) - \mathbf{b}^a(t) - \pmb{\eta}^{ad}(t))\Delta t^2 \label{eq2:imu_pre_delta_p} \end{align}\]
其中上標 \(\cdot^{d}\) 表示離散（discrete），是在 \(\Delta t\) 時間段內的測量值的高斯誤差。

從現在開始，考慮離散的情況。假設 \(\Delta t\) 是 IMU 測量值的時間間隔，從第 \(i\) 個 IMU 測量值積分到第 \(j\) 個 IMU 測量值，中間一共經歷 \(j - i\) 個 \(\Delta t\) 時間段，按照公式 (\ref{eq2:imu_pre_delta_R}) (\ref{eq2:imu_pre_delta_v}) (\ref{eq2:imu_pre_delta_p})，可以從 \(i\) 時刻的狀態值積分得到 \(j\) 時刻的狀態值：
\[\begin{align} \mathbf{R}_j &= \mathbf{R}_i \prod_{k=i}^{j-1}\exp((\tilde{\mathbf{a}}_k - \mathbf{b}^g_k - \pmb{\eta}^{gd}_k)\Delta t) \ \mathbf{v}_j &= \mathbf{v}_i + \mathbf{g}\Delta t_{ij} + \sum_{k=i}^{j-1}\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t\ \mathbf{p}_j &= \mathbf{p}_i + \sum_{k=i}^{j-1}\mathbf{v}_k\Delta t + \sum_{k=i}^{j-1}\frac{1}{2}\mathbf{g}\Delta t^2 + \frac{1}{2}\sum_{k=i}^{j-1}\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t^2 \end{align}\]
其中引入了新的變量 \(\Delta t_{ij} \doteq \sum_{k = i}^{j-1}\Delta t\)。接下來定義 \(i, j\) 時刻狀態量之間的“差值”。按照“差值”的字面意思，可以直接寫出 \(\mathbf{R}_i^T\mathbf{R}_j, \mathbf{v}_j - \mathbf{v}_i, \mathbf{p}_j - \mathbf{p}_i\)，然而這個太 NAIVE，這麽寫就不是預積分了。預積分定義的狀態量差值如下：
\[\begin{align} \Delta \mathbf{R}_{ij} &\doteq \mathbf{R}_i^T \mathbf{R}_j = \prod_{k=i}^{j-1}\exp((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_k - \pmb{\eta}^{gd}_k)\Delta t) \label{eq2:imu_pre_state_diff_delta_R_ij} \\Delta \mathbf{v}_{ij} &\doteq \mathbf{R}_i^T(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) = \sum_{k=i}^{j-1}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t \label{eq2:imu_pre_state_diff_delta_v_ij} \\Delta \mathbf{p}_{ij} &\doteq \mathbf{R}_i^T(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\Delta t_{ij} - \frac{1}{2}\mathbf{g}\Delta t_{ij}^2) \notag \&= \sum_{k=i}^{j-1}(\Delta\mathbf{v}_{ik}\Delta t + \frac{1}{2}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t) \notag \&= \sum_{k=i}^{j-1}\frac{3}{2}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t \label{eq2:imu_pre_state_diff_delta_p_ij} \end{align}\]
上面定義的狀態量差值都與 \(i, j\) 時刻的狀態量 \(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i, \mathbf{R}_j, \mathbf{v}_j, \mathbf{p}_j\) 無關。考慮到上面出現了中間時刻的角速度測量漂移值 \(\mathbf{b}^g_k\) 和線加速度測量漂移值 \(\mathbf{b}^a_k\)，為方便起見，假設 $\mathbf{b}^g_k = \mathbf{b}^g_i, \mathbf{b}^a_k = \mathbf{b}^a_i, k=i, i+1, \dots, j $。因為 Bias 也是需要估計出來的，不如此方便，便會出現大量的代估計狀態量。此處假設形成的近似，限制了 IMU 預積分的時間間隔，在應用的過程中應該註意到這一點。

下一步是將高斯誤差與測量值、漂移值分隔開，以方便估計高斯誤差。由 BCH 公式可得（這個地方還是值得推導一下的，第二個等號神復雜）：
\[\begin{align} \Delta\mathbf{R}_{ij} &\simeq \prod_{k=i}^{j-1} \left[ \exp((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\Delta t) \exp(-\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t) \right] \notag \ &= \Delta\tilde{\mathbf{R}}_{ij} \prod_{k=i}^{j-1} \exp(-\Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t) \notag \ &\doteq \Delta\tilde{\mathbf{R}}_{ij} \exp(-\delta \pmb{\phi}_{ij}) \label{eq2:imu_pre_mes_state_diff_delta_R_ij} \end{align}\]
其中 \(\mathbf{J}^k_r\doteq\mathbf{J}^k_r(\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\) 是 \(\mathfrak{so}(3)\) 的右雅可比。\(\Delta\tilde{\mathbf{R}}_{ij} \doteq \prod_{k=i}^{j-1}\exp((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\Delta t)\) 是 IMU 預積分的虛擬旋轉差值測量值，\(\exp(-\delta \pmb{\phi}_{ij})\) 是對應的高斯誤差。由此，可以進一步得到 IMU 預積分的虛擬速度差值和虛擬位移差值與它們的測量值、誤差之間的關系：
\[\begin{align} \Delta\mathbf{v}_{ij} &\simeq \sum_{k=i}^{j-1} (\Delta\tilde{\mathbf{R}}_{ik}(\mathbf{I}-\delta\pmb{\phi}^{\wedge}_{ik})(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t - \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t) \notag \ &= \Delta\tilde{\mathbf{v}}_{ij} + \sum_{k=i}^{j-1} \left[\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t - \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \notag \ &\doteq \Delta\tilde{\mathbf{v}}_{ij} - \delta\mathbf{v}_{ij} \label{eq2:imu_pre_mes_state_diff_delta_v_ij} \\Delta\mathbf{p}_{ij} &\simeq \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\mathbf{I}-\delta\pmb{\phi}^{\wedge}_{ik})(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t^2 - \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2 \notag \ &= \Delta\tilde{\mathbf{p}}_{ij} + \sum_{k=i}^{j-1}\left[\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 - \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \notag \ &\doteq \Delta\tilde{\mathbf{p}}_{ij} - \delta\mathbf{p}_{ij} \label{eq2:imu_pre_mes_state_diff_delta_p_ij} \end{align}\]

整理一下虛擬測量值與其誤差:
\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij} &\doteq \prod_{k=i}^{j-1}\exp((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\Delta t) \notag \\exp(-\delta \pmb{\phi}_{ij}) &\doteq \notag \prod_{k=i}^{j-1} \exp(-\Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t) \notag \\Delta\tilde{\mathbf{v}}_{ij} &\doteq \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t \notag \-\delta \mathbf{v}_{ij} &\doteq \sum_{k=i}^{j-1} \left[\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t - \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \notag \\Delta\tilde{\mathbf{p}}_{ij} &\doteq \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t^2 \notag \-\delta \mathbf{p}_{ij} &\doteq \sum_{k=i}^{j-1}\left[\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 - \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \notag \\end{align} \]

將公式 (\ref{eq2:imu_pre_mes_state_diff_delta_R_ij}) (\ref{eq2:imu_pre_mes_state_diff_delta_v_ij}) (\ref{eq2:imu_pre_mes_state_diff_delta_p_ij})分別代入公式 (\ref{eq2:imu_pre_state_diff_delta_R_ij}) (\ref{eq2:imu_pre_state_diff_delta_v_ij}) (\ref{eq2:imu_pre_state_diff_delta_p_ij})，得到虛擬狀態量差值測量值與 \(i, j\) 時刻狀態量的聯系:
\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij} &= \mathbf{R}^T_i\mathbf{R}_j\exp(\delta\pmb{\phi}_{ij}) \ \Delta\tilde{\mathbf{v}}_{ij} &= \mathbf{R}^T_i(\mathbf{v}_j-\mathbf{v}_i-\mathbf{g}\Delta t_{ij}) + \delta\mathbf{v}_{ij} \ \Delta\tilde{\mathbf{p}}_{ij} &= \mathbf{R}^T_i(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t^2_{ij}) + \delta \mathbf{p}_{ij} \end{align}\]
優化通過調整 \(i, j\) 時刻的狀態量 \(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i, \mathbf{R}_j, \mathbf{v}_j, \mathbf{p}_j\)，最小化 \(\delta \pmb{\phi}_{ij}, \delta \mathbf{v}_{ij}, \delta \mathbf{p}_{ij}\)。

以上 \(\Delta\tilde{\mathbf{R}}_{ij}, \Delta\tilde{\mathbf{v}}_{ij}, \Delta\tilde{\mathbf{p}}_{ij}\) 雖然與 \(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i, \mathbf{R}_j, \mathbf{v}_j, \mathbf{p}_j\) 無關，但與 \(i\) 時刻的漂移值 \(\mathbf{b}^g_i, \mathbf{b}^a_i\) 相關，這兩個漂移值也是需要估計的狀態量，在優化過程中會發生改變，使得需要重新積分。為了解決這個問題，IMU 預積分使用泰勒展開進行近似。如果漂移值的變化量過大，則需要重新積分。在漂移值處的泰勒展開形式如下：

\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij}(\mathbf{b}^g_i) &\simeq \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i)\exp\left(\frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i\right) \ \Delta\tilde{\mathbf{v}}_{ij}(\mathbf{b}^g_i, \mathbf{b}^a_i) &\simeq \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \ \Delta\tilde{\mathbf{p}}_{ij}(\mathbf{b}^g_i, \mathbf{b}^a_i) &\simeq \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \end{align}\]

得到虛擬測量值的誤差計算方法：
\[\begin{align} \mathbf{r}_{\Delta\mathbf{R}_{ij}} &\doteq -\delta \pmb{\phi}_{ij} = \ln\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \exp\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}^T_i\mathbf{R}_j \right) \ \mathbf{r}_{\Delta\mathbf{v}_{ij}} &\doteq -\delta \mathbf{v}_{ij} = \mathbf{R}^T_i(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) \notag \ &- \left( \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \right) \ \mathbf{r}_{\Delta\mathbf{p}_{ij}} &\doteq -\delta \mathbf{p}_{ij} = \mathbf{R}^T_i(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t^2_{ij}) \notag \ &- \left( \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \right) \end{align}\]

Covariances

Jacobians

IMU 預積分推導