PO-VINS: An Efficient and Robust Pose-Only Visual-Inertial State Estimator With LiDAR Enhancement

Abstract: The pose adjustment (PA) with a pose-only visual representation has been proven equivalent to the bundle adjustment (BA), while significantly improving the computational efficiency. However, the pose-only solution has not yet been properly considered in a tightly-coupled visual-inertial state estimator (VISE) with a normal configuration for real-time navigation. In this study, we propose a tightly-coupled LiDAR-enhanced VISE, named PO-VINS, with a full pose-only form for visual and LiDAR-depth measurements. Based on the pose-only visual representation, we derive the analytical depth uncertainty, which is then employed for rejecting LiDAR depth outliers. Besides, we propose a multi-state constraint (MSC)-based LiDAR-depth measurement model with a pose-only form, to balance efficiency and robustness. The pose-only visual and LiDAR-depth measurements and the IMU-preintegration measurements are tightly integrated under the factor graph optimization framework to perform efficient and accurate state estimation. Exhaustive experimental results on private and public datasets indicate that the proposed PO-VINS yields improved or comparable accuracy to sate-of-the-art methods. Compared to the baseline method LE-VINS, the state-estimation efficiency of PO-VINS is improved by 33% and 56% on the laptop PC and the onboard ARM computer, respectively. Besides, PO-VINS yields higher accuracy and robustness than LE-VINS by employing the proposed uncertainty-based outlier-culling method and the MSC-based measurement model for LiDAR depth.