10 January 2023 in IEEE/ASME Transactions on Mechatronics
In this article, we extend the invariant extended Kalman filter (EKF) to light detection and ranging (LiDAR)-inertial odometry and mapping systems using invariant observer design and the theory of Lie groups for directly fusing LiDAR and inertial measurement unit (IMU) measurements. We consider this from two different aspects and implement two independent systems. Specifically, we propose a robo-centric invariant EKF LiDAR-inertial odometry termed Inv-LIO1. Its mapping module is an ordinary used one and two modules run in separate threads. A world-centric invariant EKF LiDAR-inertial odometry termed Inv-LIO2 is designed and implemented, which has an integrated odometry and mapping architecture. In Inv-LIO1, the output of the filter is the pose estimated by the scan-to-scan match method, which serves as the initial estimate of the mapping module that refines the odometry and constructs a 3-D map. The robo-centric formulation represents that the state in a local frame shifted at every LiDAR time to prevent filter divergence. Inv-LIO2 directly fuses LiDAR feature points and IMU data to obtain the map-refined odometry by scan-to-map match method, followed by global map update. To validate the effectiveness and robustness of the proposed method, we conduct extensive experiments in various indoor and outdoor environments. Overall, Inv-LIO1 offers pure odometry with higher accuracy than other state-of-the-art systems, improving the overall performance. Inv-LIO2 achieves superior accuracy over other state-of-the-art systems in the map-refined odometry comparison.
3 April 2023 in IEEE Transactions on Intelligent Vehicles
Pose graph optimization helps reduce drift accumulated in pure odometry of visual simultaneous localization and mapping (SLAM) systems by solving a nonlinear least square problem, including both sequential constraints and loop-closing constraints. However, the covariances of all constraints are set to constant matrices or by manual setting. In this paper, we propose a novel approach to approximate covariances of constraints in pose graph optimization to better represent the true uncertainty of the underlying visual-inertial navigation system (VINS) that fuses inertial measurements and visual observations. Specifically, for sequential constraints, we propose to utilize nonlinear factor recovery to optimally extract covariance matrices from the accumulated visual-inertial odometry (VIO). For loop-closing constraints, we propose a dynamic scale estimation method to approximate the scales of the information matrices. To evaluate the effectiveness and robustness of the proposed method, we conduct extensive experiments on public and self-collected datasets in various environments. Results show that our proposed method achieves higher accuracy compared with naively-formulated pose graph optimization adopted by several state-of-the-art visual-inertial navigation systems.