A Parallel Nonlinear Factor Recovery Method for VIO Based on a Factor Graph

Under factor graph-based visual-inertial odometry (VIO), is usually used to limit the continuously increasing numbers of nodes (states) and edges (observations). However, this approach will destroy the original sparse structure of graph optimization. To solve this problem, we propose a paralleled online sparsification method in the fixed-lag smoothing VIO by minimizing Kullback–Leibler divergence (KLD) to recover nonlinear and sparse factors that optimally approximate the original dense prior induced by marginalization. In addition, we propose a new graph formulation capable of simplifying partial VIO into visual odometry (VO) with a gravity-aligned pose prior. In addition, a novel method is presented in the loop-closure phase and mapping, in which the recovered factors and the observation of landmarks can be reused for more consistent back-end optimization. The proposed method is validated based on real datasets and compared with other state-of-the-art methods to verify its efficiency and accuracy. The experimental results demonstrate that the proposed sparse method effectively reduces computational complexity when compared to the traditional tightly coupled VINS system. Furthermore, the proposed method offers a novel perspective for achieving global consistent mapping, making it an innovative addition to existing advanced systems employing sparse steps.