Robust Visual-Inertial Odometry Based on a Kalman Filter and Factor Graph

We present a real-time, high-accuracy, robust, tightly coupled visual-inertial odometry (VIO) algorithm, including monocular-inertial odometry and stereo-inertial odometry, and uses inertial measurement unit (IMU) pre-integration that is based on fourth-order Runge–Kutta (PK4) and IMU initialization based on maximum a posteriori (MAP) estimation. In particular, we used the multi-state constraint Kalman filter (MSCKF) to fuse vision and IMU measurement data for state estimation. In the optimization stage, we simultaneously considered and optimized all of the historical constraints, and performed multiple iterations to reduce the linearity errors. For further reducing the cumulative error and improving the relocation accuracy, we used a bag-of-words model for global optimization. To lower the computational cost and increase the real-time performance, we set keyframe insertion mechanism and introduced sliding window, and used a new form of Kalman gain that converts the Kalman gain in multi-state constraint Kalman filtering into the inverse of the state dimension. We validated the proposed method by using the EuRoC MAV dataset and KITTI dataset. We performed physics experiments in an outdoor environment with unstable light, to further validate the accuracy and robustness of our method.