An Integrated INS/Lidar Odometry/Polarized Camera Pose Estimation via Factor Graph Optimization for Sparse Environment
Tao Du, Shuyao Shi, Yunhao Zeng, Jian Yang, Lei Guo
Abstract
For the outdoor scenes with sparse features, the performance of inertial navigation system (INS)/Lidar odometry (LO)-based simultaneous localization and mapping (SLAM) pose estimation is drastically affected by the accumulated drift error and sparse features. A polarized camera (Polcam) that can obtain the absolute yaw angle from the polarized skylight is introduced to compensate for the accumulated drift error. A yaw measurement model based on the polarized camera is derived, and it will not suffer from the sparse and GNSS-denied environment. Moreover, it is integrated with INS/LO to construct a factor graph. The factor graph that consists of INS factor, polarized camera factor, lidar odometer factor, and motion model factor is optimized by nonlinear least squares. For the sparse environmental features, a robust and effective method named correntropy-based bidirectional generalized ICP (CoBigICP) has been adopted to register the sparse point cloud data. A loosely coupled INS/LO/Polcam integration based on factor graph optimization for sparse scenes is proposed, and it is tested and validated by an open dataset and experiment in the real-world environment, respectively. Different integration modes with different register methods are compared both in the simulation and experiment. The results show that the INS/LO integration with Polcam via factor graph optimization can realize better pose estimation accuracy than without in a sparse environment.