Accurate Initialization Method for Monocular Visual-Inertial SLAM

This In robotics, visual-inertial sensor fusion has become one of the most active research topics; optimization-based fusion approaches have gone beyond filtering approaches in terms of robustness and accuracy. For the optimization-based visual-inertial Simultaneous Localization and Mapping (SLAM), accurate initialization is essential for this nonlinear system which requires an accurate estimation of the initial states (Inertial Measurement Unit (IMU) biases, scale, gravity, and velocity). Therefore, our goal is to propose a more robust initialization method. First, we estimate the gyroscope bias, initial scale, and gravity. Then, we use the gravity magnitude to refine the gravity direction by minimizing the error state on the tangent space of the estimated gravity vector. After that, we estimate the accelerometer bias separately from gravity. Finally, based on the condition number and convergence process, we propose a robust and automatic termination criterion to indicate when the initialization is successfully achieved. Additionally, we use all the initial estimated values to initialize a visual-inertial SLAM system. We test our initialization method with different sequences of the public EuRoC dataset, and real-time hand-held experiment in indoor and outdoor environments. The results demonstrated good performance of both the estimated initial state and the automatic termination criterion. They also illustrated that the estimated gravity converges within a short time interval using the refinement approach.