Fast and Accurate Initialization for Monocular Vision/INS/GNSS Integrated System on Land Vehicle

Abstract

Initialization is a fundamental task for the state estimation of multi-sensor integration. In this paper, we propose a novel approach to achieve a fast and accurate initialization for the integrated system. With the outputs from GNSS, the rough roll, pitch, yaw, and direction of gravity are calculated. Based on these initial states, the GNSS/INS fusion and Visual-Inertial Odometry (VIO) are simultaneously launched whenever the observations are available. Subsequently, a coarse scale is retrieved by assigning the initial states of GNSS/INS fusion to the VIO procedure. The scale can be utilized to get poses and points very close to the real world. Once the VIO estimation converges, the transformation parameters between the poses of VIO and GNSS/INS fusion are estimated via non-linear optimization. Thus, we can align these two trajectories and evaluate the scale error. Through six vehicular tests, we analyze the performance of the proposed method with different illumination, obstructions, and motion patterns, and compare the results of the proposed approach with the state-of-the-art algorithms. The results indicated that the Visual-Inertial parameters converge within 9 seconds in our method. The scale errors of both our method are mostly within 10%. The alignment errors are mostly at centimeter-level in our method, which is comparable to the existing approaches.