Efficient Right-Decoupled Composite Manifold Optimization for Visual Inertial Odometry

Abstract

A composite manifold is defined as a concatenation of noninteracting manifolds, which may experience some loss of accuracy and consistency when propagating IMU dynamics based on Lie theory. However, from the perspective of ordinary differential equation modeling in dynamics, they demonstrate similar convergence rates and reduced computational complexity in iterative manifold optimization. In this context, this paper proposes a right decoupled composite manifold $\left(\mathrm{SO}\left(3\right),{ℝ}^3,{ℝ}^3\right)$ for visual-inertial sliding-window iterative optimization compared with other manifolds including chained translation and rotation $\left(\mathrm{SO}\left(3\right)\times {ℝ}^3,{ℝ}^3\right)$, special Euclidean group $\left(\mathrm{SE}\left(3\right),{ℝ}^3\right)$, and extended pose ${\mathrm{SE}}_2\left(3\right)$ concerning the orientation, position, and velocity estimation. Furthermore, the inertial measurement unit (IMU) dynamics is propagated through extended pose ${\mathrm{SE}}_2\left(3\right)$ with half rotation to maintain the accuracy of IMU preintegration. Moreover, to enhance robustness, a robustified Cauchy loss function is employed. The proposed method is evaluated with simulation and experiments on static and more challenging dynamic environments, considering its accuracy, efficiency, and robustness. Additionally, all necessary Jacobians for visual reprojection residuals and IMU preintegration residuals are provided in analytical form with numerical verification.