给 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}\]