  This paper presents a range inertial localization algorithm for a 3D prior
map. The proposed algorithm tightly couples scan-to-scan and scan-to-map point
cloud registration factors along with IMU factors on a sliding window factor
graph. The tight coupling of the scan-to-scan and scan-to-map registration
factors enables a smooth fusion of sensor ego-motion estimation and map-based
trajectory correction that results in robust tracking of the sensor pose under
severe point cloud degeneration and defective regions in a map. We also propose
an initial sensor state estimation algorithm that robustly estimates the
gravity direction and IMU state and helps perform global localization in 3- or
4-DoF for system initialization without prior position information.
Experimental results show that the proposed method outperforms existing
state-of-the-art methods in extremely severe situations where the point cloud
data becomes degenerate, there are momentary sensor interruptions, or the
sensor moves along the map boundary or into unmapped regions.
