On-Manifold Preintegration for Real-Time Visual--Inertial Odometry

Abstract

Abstract-Current approaches for visual-inertial odometry (VIO) are able to attain highly accurate state estimation via nonlinear optimization. However, real-time optimization quickly becomes infeasible as the trajectory grows over time; this problem is further emphasized by the fact that inertial measurements come at high rate, hence leading to fast growth of the number of variables in the optimization. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes into single relative motion constraints. Our first contribution is a preintegration theory that properly addresses the manifold structure of the rotation group. We formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the maximum a posteriori state estimator. Our theoretical development enables the computation of all necessary Jacobians for the optimization and a-posteriori bias correction in analytic form. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated into a visual-inertial pipeline under the unifying framework of factor graphs. This enables the application of incremental-smoothing algorithms and the use of a structureless model for visual measurements, which avoids optimizing over the 3D points, further accelerating the computation. We perform an extensive evaluation of our monocular VIO pipeline on real and simulated datasets. The results confirm that our modelling effort leads to accurate state estimation in real-time, outperforming state-of-the-art approaches. SUPPLEMENTARY MATERIAL• Video of the experiments: https://youtu.be/CsJkci5lfco • Source-code for preintegrated IMU and structureless vision factors https://bitbucket.org/gtborg/gtsam.

Visual-Inertial Odometry on Chip: An Algorithm-and-Hardware Co-design Approach

“…The present VIO pipeline is based on [32], adopting a factor graph model to perform inference on the robot state, and using preintegrated IMU factors and structureless vision factors to model the data from the inertial measurement unit and the onboard cameras. However, contrary to [32], our approach works with both monocular and stereo camera measurements.…”

“…However, contrary to [32], our approach works with both monocular and stereo camera measurements. The approach can include data from the right camera when available, and falls back to a monocular approach otherwise.…”

“…(See [32] for details.) Considering two consecutive keyframes at time i and j, the IMU preintegration performs Euler integration of the IMU acceleration and gyroscope measurements (a k , ω k ) for all sampling times k = i, .…”

“…The MAP estimator computes the state estimate by solving a nonlinear least squares problem, which assumes the form [32]:…”

“…The visual inertial odometry (VIO) literature is vast, including approaches based on filtering [14][15][16][17][18][19], fixed-lag smoothing [20][21][22][23][24], full smoothing [25][26][27][28][29][30][31][32]. The algorithms considered here are related to IMU preintegration models [30][31][32][33].…”