Tightly Coupled Range Inertial Localization on a 3D Prior Map Based on Sliding Window Factor Graph Optimization

TL;DR

This work tackles map-based localization under challenging conditions where point clouds degenerate or the sensor enters unmapped regions. It introduces a tightly coupled range inertial localization framework that jointly optimizes scan-to-scan, scan-to-map, and IMU factors within a sliding-window graph, bounded by marginalization for real-time operation. A robust gravity-direction initialization enables 3- or 4-DoF global localization without prior pose, while experimental results show superior resilience to degeneration and interruptions compared with state-of-the-art baselines. The approach yields accurate, continuous pose estimates in both indoor and outdoor, demonstrating practical robustness for navigating across mapped and unmapped areas with real-time performance.

Abstract

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.

Figures (6)

• Figure 1: Indoor localization experiment using a Microsoft Azure Kinect. The proposed method enables robust and accurate pose estimation in challenging situations including under quick sensor motions, complete degeneration of point clouds, and traveling across both mapped and unmapped regions. The color of the map point cloud indicates the height of each point.
• Figure 2: Proposed factor graph structure. (a) The proposed factor graph consists of tightly coupled scan-to-scan and scan-to-map registration factors along with IMU factors. Old frames that leave the optimization window are marginalized to bound the computation cost. The proposed method enables robust pose estimation in challenging situations including (b) the sensor travels over unmapped places, and (c) point cloud data becomes degenerated.
• Figure 3: Estimated trajectories for the Hard sequence. Existing methods suffered from unmapped regions and severe degeneration of point clouds. The proposed method successfully continued tracking the sensor pose under these severe conditions thanks to its tightly coupled registration factors and windowed state optimization.
• Figure 4: Outdoor experimental environment (280 $\times$ 200 $\text{m}^2$). The evaluation sequences include four challenging situations: A) large translational and rotational movements, B) point cloud data interruptions, C) aggressive sensor rotations, and D) traveling over unmapped regions. In particular, in the B regions, the sensor was completely occluded several times, which created extreme challenges to localization methods.
• Figure 5: Automatic initialization result. (a) The gravity direction of the LiDAR scans was estimated, and (b) the 2D global localization successfully estimated the initial sensor position.