Accurate LiDAR-Camera Fused Odometry and RGB-Colored Mapping

Due to the complementary properties between sensors, multi-sensor fusion can effectively promote accuracy and tackle the challenging scenes in simultaneous localization and mapping (SLAM) tasks. To this end, we propose a novel LiDAR-camera fused method for odometry and mapping using dense colored point clouds. With the camera well calibrated to the LiDAR, we can acquire colored point clouds, providing constraints of color and geometric features for SLAM tasks. Our LiDAR-camera fused odometry and mapping system leverages the geometric features from the point cloud and color information from the camera. The main innovation is projecting the colored point to the local point cloud plane and formulating an RGB color objective function for SLAM tasks. We optimize the geometric and color objective functions jointly to estimate the precise pose of the robot. In particular, we maintain a color feature map and a planar feature map separately in the optimization process, which reduces the algorithm's computation significantly. The evaluation experiments are performed on a UGV platform and a handheld platform. We demonstrate the effectiveness of our LiDAR-camera fusion method using the solid-state LiDAR and camera on an Intel RealSense L515 sensor. The results show that our method effectively promotes localization accuracy, works well in challenging environments, and outperforms existing methods. We will share the code publicly to benefit the community (after the review stage).