Tightly-Coupled Lidar-Visual-Inertial Odometry and Mapping in Real Time

We proposed a tightly-coupled Lidar-visual-inertial odometry and mapping method, which takes advantage of measurement of Lidar, visual and inertial sensors to achieve highly accurate, real-time 6DoF state estimation and map-building in GNSS-denied environments. The proposed odometry is a tightly-coupled optimization-based method, obtains robust and low drift odometry by fusing pre-integrated IMU measurements, visual features from the image, and geometric features from Lidar data. Further, we adapt an online method to mitigate degeneracy in optimization problems to improve robustness in environmentally degenerate cases. Simulation and real-world experiments show that the proposed method exhibits similar or better robustness and accuracy with the state-of-the-art SLAM methods.