A factor graph optimization mapping based on normaldistributions transform

Abstract

This paper aims to achieve highly accurate mapping results and real time pose estimation of autonomous vehicle by using the normal distribution transform (NDT) algoritm. A factor graph optimization algorithm (FGO-NDT) is proposed to address the poor real-time performance and pose drift errors of the NDT algorithm. Smooth point cloud data are obtained by multisensor calibration and data preprocessing. NDT registration is then used for lidar odometry and feature matching. The global navigation satellite system (GNSS) data and loop detection results are added to the factor graph framework as the pose constraint factors to optimize the pose trajectory and eliminate the pose drift error generated during mapping. In addition, a sliding window method is used for map registration to extract a local map to shorten the map loading time. Thus, the real-time performance of creating point cloud maps of large scenes is significantly improved. Several experiments are conducted in different environments to verify the accuracy and performance of the FGO-NDT. The experimental results demonstrate that the proposed method can eliminate the pose estimation error caused by drift, improve the local structure, and reduce and root mean square error.