LIVIO: A High-Precision Lidar-Visual-Inertial Odometry via Filtering and Optimization

We propose a synergistic lidar-visual-inertial state estimation system, lidar-visual-inertial odometry (LIVIO), which achieves high-accuracy and robust localization results. LIVIO is composed of two sub-systems: a lidar-inertial system (LIS) based on iterated error-state Kalman filter (IESKF) and a visual-inertial system (VIS) based on nonlinear optimization. In the prediction stage of LIS, a midpoint integration approach is used to obtain the discrete states and covariance propagation, significantly enhancing pose estimation accuracy. For the update stage, LIS employs normal distribution transform (NDT) registration coupled with sub-maps for efficient map matching, resulting in lower time consumption. On the other hand, to improve the localization accuracy, VIS strategically integrates LIS odometry as a prior constraint in its factor graph. Another key innovation of ours lies in the novel feature velocity factor, establishing a geometric constraint between the 3-D velocity in the camera frame and its corresponding projected 2-D velocity on the normalized image plane. This formulation effectively bounds localization drift through physical motion consistency. Finally, we validate our proposed LIVIO on both public and collected datasets. Our method achieves relative pose errors (RPEs) below 0.253 m and time consumption under 60 ms across all datasets, outperforming state-of-the-art localization systems in nearly all evaluated scenarios. The experimental results demonstrate that LIVIO is accurate, robust, real-time, and highly applicable.