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

Current approaches for visual-inertial odometry (VIO) are able to attain\nhighly accurate state estimation via nonlinear optimization. However, real-time\noptimization quickly becomes infeasible as the trajectory grows over time, this\nproblem is further emphasized by the fact that inertial measurements come at\nhigh rate, hence leading to fast growth of the number of variables in the\noptimization. In this paper, we address this issue by preintegrating inertial\nmeasurements between selected keyframes into single relative motion\nconstraints. Our first contribution is a \\emph{preintegration theory} that\nproperly addresses the manifold structure of the rotation group. We formally\ndiscuss the generative measurement model as well as the nature of the rotation\nnoise and derive the expression for the \\emph{maximum a posteriori} state\nestimator. Our theoretical development enables the computation of all necessary\nJacobians for the optimization and a-posteriori bias correction in analytic\nform. The second contribution is to show that the preintegrated IMU model can\nbe seamlessly integrated into a visual-inertial pipeline under the unifying\nframework of factor graphs. This enables the application of\nincremental-smoothing algorithms and the use of a \\emph{structureless} model\nfor visual measurements, which avoids optimizing over the 3D points, further\naccelerating the computation. We perform an extensive evaluation of our\nmonocular \\VIO pipeline on real and simulated datasets. The results confirm\nthat our modelling effort leads to accurate state estimation in real-time,\noutperforming state-of-the-art approaches.\n