LiDAR and Inertial Fusion for Pose Estimation by Non-linear Optimization

Pose estimation purely based on 3D point-cloud could suffer from degradation, e.g. scan blocks or scans in repetitive environments. To deal with this problem, we propose an approach for fusing 3D spinning LiDAR and IMU to estimate the ego-motion of the sensor body. The main idea of our work is to optimize the poses and states of two kinds of sensors with non-linear optimization methods. On the one hand, a bunch of IMU measurements are considered as a relative constraint using pre-integration and the state errors can be minimized with the help of laser pose estimation and non-linear optimization algorithms; on the other hand, the optimized IMU pose outputs can provide a better initial for the subsequent point-cloud matching. The method is evaluated under both simulation and real tests with comparison to the state-of-the-art. The results show that the proposed method can provide better pose estimation performance even in the degradation cases.