Visual-inertial navigation, mapping and localization: A scalable real-time causal approach

Abstract

We describe a model to estimate motion from monocular visual and inertial measurements. We analyze the model and characterize the conditions under which its state is observable, and its parameters are identifiable. These include the unknown gravity vector, and the unknown transformation between the camera coordinate frame and the inertial unit. We show that it is possible to estimate both state and parameters as part of an on-line procedure, but only provided that the motion sequence is ‘rich enough’, a condition that we characterize explicitly. We then describe an efficient implementation of a filter to estimate the state and parameters of this model, including gravity and camera-to-inertial calibration. It runs in real-time on an embedded platform. We report experiments of continuous operation, without failures, re-initialization, or re-calibration, on paths of length up to 30 km. We also describe an integrated approach to ‘loop-closure’, that is the recognition of previously seen locations and the topological re-adjustment of the traveled path. It represents visual features relative to the global orientation reference provided by the gravity vector estimated by the filter, and relative to the scale provided by their known position within the map; these features are organized into ‘locations’ defined by visibility constraints, represented in a topological graph, where loop-closure can be performed without the need to re-compute past trajectories or perform bundle adjustment. The software infrastructure as well as the embedded platform is described in detail in a previous technical report.