In this work, we address two current challenges in real-time visual-inertial odometry (VIO) systems — efficiency and nonlinearity. To this end, we present a novel approach to tightly couple visual and inertial measurements in a fixed-lag VIO framework using information sparsification. To bound computational complexity, fixed-lag smoothers perform marginalization of variables but consequently deteriorate accuracy and especially efficiency. Current state-of-the-art approaches work around this by selectively discarding measurements and marginalizing additional variables. However, such strategies are sub-optimal from an information-theoretic perspective. In contrast, our approach formulates an optimization based on Kullback-Leibler divergence to preserve most of the information. To validate our approach, we conduct extensive real-time drone tests and perform comparisons to current state-of-the-art fixed-lag VIO methods in the EuRoC visual-inertial dataset. The experimental results show that the proposed method achieves competitive and superior accuracy in almost all trials. In achieving a more efficient and accurate state estimator, the second part of the work presents the on-going progress in formulating an optimization-based VIO system using matrix Lie groups. Inspired by the recently developed Invariant-EKF framework, the proposed framework presents better convergence and addresses the consistency problem commonly seen in EKF-based and fixed-lag frameworks. In particular, we provide detailed derivations of a novel IMU preintegration framework using the
group affine properties. Simulation results show our proposed formulation allows the nonlinear optimizer to converge with significantly fewer iterations, as compared to the
state-of-the-art IMU preintegration scheme.