Vehicle Odometry with Camera-Lidar-IMU Information Fusion and Factor-Graph Optimization

Formula Student Driverless (FSD) requires students to design and build a driverless vehicle to race on track, which incurs great demands on the odometry solution. High accuracy odometry plays a significant role in Simultaneous Localization and Mapping (SLAM) and automatic navigation mission. This paper proposes an odometry method based on Camera-Lidar-IMU information fusion and Factor-Graph optimization. It solves the problem of observation of speed and pose transformation in high speed racing scenes with sparse features. Firstly, a YoloV3-tiny object detector is used to identify cone objects captured through camera sensor, which is used to segment the object points from the Lidar pointcloud. Then, the object points are registered by utilizing the inertial measurement unit (IMU) pre-integration result as rough estimation, to obtain increment of pose transformation in horizontal plane. And a Ground Normal Vector Registration method is developed using ground points to solve increment of vertical pose transformation. These two transformation results are coupled to get a real-time odometry. At last, the odometry results and observations are optimized at the back-end with the Factor-Graph algorithm. Experiments show that the method presented in this paper performs well in real environment, and achieves high accuracy and provides a good reference for vehicle SLAM and navigation.