Forster预积分论文On-Manifold Preintegration for Real-Time Visual-Inertial Odometry公式推导

公式推导

根据等式（10）

\[R\ Exp(\phi)\ R^{T} = exp(R\phi^{\hat{}}R^{T}) = Exp(R\phi) \]

推导等式（11）

\[\begin{align*} Exp(\phi)R &= RR^{T}Exp(\phi)R \\ &= R(R^{T}Exp(\phi)R) \\ &= RExp(R^{T}\phi) \end{align*} \]

等式（35）部分推导

\[\prod_{k=i}^{j-1} \left[ Exp((\tilde{\omega}_k - b_i^g)\Delta t) Exp(-J_r^k \eta_k^{gd} \Delta t) \right] \\ \]

令\(Exp((\tilde{\omega}_k - b_i^g)\Delta t) = R_k\)，\(Exp(-J_r^k \eta_k^{gd} \Delta t) = Exp(\phi_k)\)

\[\begin{align*} \prod_{k=i}^{j-1} R_k Exp(\phi_k) &= R_iExp(\phi_i) R_{i+1}Exp(\phi_{i+1}) \cdots R_{j-2}Exp(\phi_{j-2}) R_{j-1}Exp(\phi_{j-1}) \\ &= R_iExp(\phi_i) R_{i+1}Exp(\phi_{i+1}) \cdots R_{j-2}(Exp(\phi_{j-2}) R_{j-1})Exp(\phi_{j-1}) \end{align*} \]

根据等式（11）括号中\((Exp(\phi_{j-2}) R_{j-1}) = R_{j-1} Exp(R^{T}_{j-1} \phi_{j-2})\)

\[\begin{align*} \prod_{k=i}^{j-1} R_k Exp(\phi_k) &= R_iExp(\phi_i) R_{i+1}Exp(\phi_{i+1}) \cdots R_{j-2}R_{j-1} Exp(R^{T}_{j-1} \phi_{j-2})Exp(\phi_{j-1}) \\ &= R_iExp(\phi_i) R_{i+1}Exp(\phi_{i+1}) \cdots R_{j-3}Exp(\phi_{j-3}) R_{j-2}R_{j-1} Exp(R^{T}_{j-1} \phi_{j-2})Exp(\phi_{j-1}) \\ &= R_iExp(\phi_i) R_{i+1}Exp(\phi_{i+1}) \cdots R_{j-3}R_{j-2}R_{j-1} Exp((R_{j-2}R_{j-1})^{T}\phi_{j-3}) Exp(R^{T}_{j-1} \phi_{j-2})Exp(\phi_{j-1}) \\ &\cdots \\ &= \Delta\tilde{R}_{ij} \prod_{k=i}^{j-1} Exp(- \Delta\tilde{R}_{k+1j} J_r^k \eta_k^{gd} \Delta t) \\ &= \Delta\tilde{R}_{ij} Exp(- \delta \phi_{ij}) \end{align*} \]

论文理解

等式（33）中，论文提到\(\Delta v_{ij}\)和\(\Delta p_{ij}\)与\(\Delta R_{ij}\)不同，它们不表示两时刻之间实际的速度和位置的物理变化。它们只是独立于时刻\(i\)和重力影响的一种预积分的表达形式，可以直接从IMU的观测中得到。