Invariant Filter Based Preintegration for Addressing the Visual Observability Problem

tightly coupled Visual Inertial Navigation Systems (VINS) implementations have proven their superiority due to their ability to jointly optimize all state variables. While many approaches employ an Extended Kalman Filter (EKF) for optimization given their simplicity and efficiency, they suffer from inconsistencies associated with state uncertainty. This is due to the observability of the system which is not invariant for all transformations when state is filtered through an EKF. In this paper, we address this issue through the use of an Invariant Extended Kalman Filter (I-EKF) to address the observability challenge. We derive an IMU factor that does not depend on the assumption that the biases are the same between two sequential keyframes and that the VINS, when given the preintegrated measurements, does not have to separately estimate the state variables. After integrating our algorithm with the open-source parallel tracking and mapping system ORB-SLAM [1], experimental results confirm that our derivation is computationally efficient while demonstrating superior accuracy compared to other state-of-the-art filtering and graph optimization based VINS.