FT-LVIO: Fully Tightly coupled LiDAR-Visual-Inertial odometry

In this paper, we propose a fully tightly-coupled multi-sensor fusion framework termed FT-LVIO, that fuses measurements from a light detection and ranging (LiDAR), a monocular camera and an inertial measurement unit (IMU) simultaneously to achieve robust and accurate state estimation in real time. FT-LVIO is built atop the framework of an error-state-iterated Kalman filter. To take full advantage of the complimentary characteristics of individual sensors, LiDAR point clouds are undistorted by IMU prediction to the nearest camera exposure time and the filter is updated with measurements from all sensors. In addition, an efficient sampling method for the LiDAR point-to-plane measurements is proposed, which can help select the measurements providing sufficient constraints to the pose estimation and facilitate a low-drift odometry. Extensive experiments are performed on both the public NTU dataset and the private handheld dataset, and the results show that the proposed FT-LVIO outperforms the state-of-the-art LiDAR-inertial, visual-inertial and LiDAR-visual-inertial methods in both accuracy and robustness. Furthermore, FT-LVIO can survive in the challenging staircase environment.