PaperlayerView exact revision on arXiv

Source-linked AI summary

GLIO2: A GPU-Parallelized Tightly-Coupled LiDAR-Inertial-GNSS System for Robust and Real-Time Global Localization and Mapping

Qi Zhang, Xikun Liu, Qijun Qin, Xiangru Wang, Junzhe Wang, Naigui Xiao, Jianhao Jiao, Weisong Wen

arXiv:2610.12411v1cs.RO

TL;DR

Existing LiDAR fusion can reduce scans to fixed pose constraints, leaving raw correspondences unavailable for later revision. GLIO2 jointly optimizes LiDAR, IMU, and GNSS measurements online and reuses cached factors offline. Across evaluated sequences, batch GLIO2 achieved the lowest 3D ATE RMSE among evaluated systems.

  • Problem

    Existing scan-to-map methods reduce each scan to a fixed LiDAR constraint rather than retaining raw correspondences for re-association and relinearization.

    Source: §I · p. 3

  • Method

    GLIO2 uses a GPU-parallel sliding-window graph to jointly optimize dense scan-to-multiscan LiDAR, IMU pre-integration, and raw GNSS, then reuses cached factors for offline batch refinement.

    Source: §I · p. 3, §VII · p. 20

  • Results

    Across evaluated sequences, batch GLIO2 achieved the lowest 3D ATE RMSE among evaluated systems; online bridge accuracy was 1.6 m horizontal while evaluated LiDAR-based baselines diverged.

    Source: §I · p. 3, §VII · p. 20

  • Takeaways & Limitations

    On an NVIDIA Jetson Orin NX, the full pipeline averages 39.60–55.54 ms per scan, and the offline back-end re-optimizes a 30-minute session in about 24 seconds.

    Source: §VII · p. 20

    Both GNSS configurations depend on adequate startup measurements; poor initial GNSS conditions or initialization during dynamic motion degrades the fused estimate.

    Source: §VII · p. 21

Abstract

from arXiv · show

Globally consistent, real-time state estimation in large-scale, perceptually degraded environments is essential for autonomous vehicles and aerial robots, and requires fusing LiDAR, inertial, and GNSS measurements. Existing fusion methods, however, share a scan-to-map front-end with two failure modes. First, each scan is aligned to an incrementally built map that drifts under degeneracy, and once the estimate diverges the error is irrecoverable. Second, even without divergence, a registration biased by dynamic objects or wrong correspondences is propagated as a single pose constraint with an over-confident covariance, leaving its correspondences unavailable for GNSS to re-weight or relinearize. We propose GLIO2, a tightly-coupled LiDAR-Inertial-GNSS system whose GPU-parallel front-end jointly optimizes scan-to-multiscan LiDAR, IMU pre-integration, and raw GNSS measurements in a single sliding-window factor graph, sustaining real-time operation on edge hardware. A complementary offline back-end reuses the same cached factors to refine the entire trajectory in batch, completing the 30-min, 4.51-km UrbanNav Whampoa sequence in about 24 s. Across three public benchmarks (UrbanNav, MARS-LVIG, M3DGR) and self-collected UAV and vehicle data, GLIO2 attains the best overall accuracy among evaluated systems. On a 5.66-km bridge traversed at up to 96 km/h, where every competing baseline diverges under LiDAR degeneracy, it maintains 1.6 m horizontal accuracy. On an NVIDIA Jetson Orin NX, the full pipeline runs at about 25 Hz (39.60 ms per scan). The source code and datasets will be released.

I. Introduction

GLIO2 targets globally consistent, locally smooth, robust, real-time state estimation by tightly coupling LiDAR, inertial, and GNSS measurements. The introduction describes the modalities’ complementary limits, the challenges of existing fusion, and the system’s reported benchmark performance.

I. Introduction

  • LiDAR–inertial odometry offers local smoothness but drifts and degenerates; GNSS bounds global drift yet is vulnerable to multipath, NLOS reception, and outages.The modalities are complementary: GNSS anchors the global frame, while LIO carries estimates through GNSS outages.Source: §I · pp. 1–2
  • Mainstream scan-to-map front ends can diverge under degeneracy, while biased fixed LiDAR pose constraints from wrong correspondences can overpower GNSS and remain uncorrected.The cited methods do not re-associate past point clouds or relinearize their residuals.Source: §I · p. 2
  • Dense scan-to-multiscan registration with raw point residuals requires repeated correspondence, covariance, and Jacobian updates, exceeding the CPU’s 100 ms budget on denser scans.The described CPU implementation peaks at 200–300 ms per scan.Source: §I · p. 2
  • GLIO2 jointly optimizes scan-to-multiscan LiDAR, raw double-differenced pseudorange and Doppler, and IMU pre-integration in a GPU-parallel sliding-window graph.Its offline back-end reuses compact relative-pose LiDAR factors generated online to refine the full trajectory.Source: §I · pp. 2–3
  • Across all eleven sequences, batch GLIO2 achieved the lowest 3D ATE RMSE among evaluated LiDAR–inertial–GNSS systems.The online front-end completed all eleven sequences.Source: §I · p. 3

A. LiDAR Registration Paradigms

  • Scan-to-map registration aligns each scan to an incrementally built map and yields one absolute pose; scan-to-multiscan instead jointly refines keyframe poses through relative constraints.The latter requires repeated re-association and relinearization as keyframe poses move.Source: A. LiDAR Registration Paradigms · p. 3

B. GNSS Observables

  • GNSS fusion approaches include single-receiver pseudorange positioning, raw pseudorange and Doppler, precise-point positioning, double-differencing, and carrier-phase methods such as RTK and TD-CP.Double-differencing cancels satellite and receiver clock errors and largely removes spatially correlated atmospheric delays over a short baseline.Source: B. GNSS Observables · pp. 3–4

C. LiDAR–Inertial–GNSS Integration Systems

This section compares GNSS coupling strategies and presents GLIO2’s joint sliding-window approach, online and offline workflow, and state representation.

C. LiDAR–Inertial–GNSS Integration Systems

  • Loosely coupled methods reduce GNSS to a position, recursive filters do not re-optimize past measurements, and hierarchical graphs freeze LiDAR registration into pose factors.Factor-graph methods keep states available for relinearization, but fixed-submap and hierarchical designs still constrain how LiDAR information is revised.Source: C. LiDAR–Inertial–GNSS Integration… · p. 4
  • GLIO combines an online scan-to-map front-end with offline scan-to-multiscan batch optimization, but the batch still needs a non-divergent initial estimate.Its back-end re-registers point clouds, making it slow.Source: C. LiDAR–Inertial–GNSS Integration… · p. 4
  • GLIO2 jointly optimizes raw double-differenced pseudorange and Doppler with dense scan-to-multiscan LiDAR, retaining relative keyframe constraints instead of fixed-map pose priors.Correspondences and residuals are re-associated and relinearized at every iteration.Source: C. LiDAR–Inertial–GNSS Integration… · p. 4

B. Notation and Coordinate Frames

  • GLIO2 uses ECEF, local-world, LiDAR, IMU, and receiver frames; left superscripts indicate expression frames, and the local-world frame begins at the trajectory start.The LiDAR and IMU frames are rigidly mounted on the platform.Source: B. Notation and Coordinate Frames · p. 5, Figure 3 · p. 6

C. System State Representation

  • Each window state combines a local-world-to-ECEF transform and gravity with navigation states containing IMU motion, inertial biases, and GNSS clock-drift rates.Gravity magnitude is fixed at 9.81 m/s^2 while its direction is estimated; double differencing cancels receiver clock offset, which is reinstated for the base-station-free variant.Source: C. System State Representation · pp. 5–6

A. System Architecture

  • The online front-end deskews and initializes scans, then solves the sliding-window graph at the LiDAR rate using GPU per-point linearization and CPU updates.The offline back-end reuses cached relative factors, applies GNSS fault detection and de-weighting, and batch-optimizes the trajectory.Source: A. System Architecture · p. 5, Figure 2 · p. 6

IV. Methodology

The factor graph jointly minimizes weighted residual costs from active sensor factors and a marginalized prior. Double-differenced pseudoranges provide geometric constraints while canceling shared receiver, satellite, and atmospheric errors.

A. Factor Graph Formulation

  • The MAP objective combines a whitened marginalized prior with weighted IMU, LiDAR, pseudorange, and Doppler residual costs across the active state.The objective assumes independent measurements with zero-mean Gaussian noise.Source: A. Factor Graph Formulation · p. 6

B. Double-Difference Pseudorange Factor

  • Double differencing first removes receiver clock bias, then removes satellite clock biases and short-baseline propagation terms, leaving geometric ranges plus noise.The figure depicts the rover, base station, master satellite, and secondary satellite used in this geometry.Source: B. Double-Difference Pseudorange Factor · p. 7, Figure 4 · p. 7
  • The master satellite is the highest-elevation satellite jointly tracked by both receivers above a 15° mask, and each other common satellite forms a double difference against it.Using one shared master gives the double-difference covariance off-diagonal terms, which are retained when whitening the residuals.Source: B. Double-Difference Pseudorange Factor · p. 7

C. Doppler Factor

The Doppler factor supplies instantaneous velocity information that double-differenced pseudorange does not provide. It uses raw single-receiver, single-satellite residuals and routes them to constellation-specific clock-drift channels.

C. Doppler Factor

  • Raw Doppler supplies an instantaneous velocity constraint, complementing double-differenced pseudorange, which is insensitive to receiver clock drift and carries little velocity information.The raw form preserves absolute receiver clock-drift information that double differencing would cancel.Source: C. Doppler Factor · p. 7
  • Each Doppler residual measures satellite–receiver range rate projected onto the line of sight, with satellite clock drift applied as a feed-forward correction.The receiver antenna velocity is obtained by differentiating its position transformation, using bias-corrected gyroscope readings.Source: C. Doppler Factor · pp. 7–8
  • Per-satellite residuals are routed to their constellation’s clock-drift channel, while a random-walk factor keeps all four channels full rank when a constellation is momentarily unobserved.The residuals are stacked by epoch with diagonal covariance.Source: C. Doppler Factor · p. 8

D. IMU Pre-Integration Factor

IMU pre-integration summarizes measurements between frames as a relative-motion constraint, while scan-to-multiscan LiDAR retains a six-degree-of-freedom global pose gauge freedom.

D. IMU Pre-Integration Factor

  • The 15-dimensional IMU pre-integration residual stacks rotation, velocity, position, and gyroscope and accelerometer bias random-walk errors, weighted by propagated covariance.Bias Jacobians allow increments to be corrected after bias updates without re-integrating; this high-rate factor constrains states between LiDAR and GNSS updates.Source: D. IMU Pre-Integration Factor · p. 8

E. LiDAR Factor via Scan-to-Multiscan Registration

  • Relative LiDAR residuals are unchanged by applying the same rigid rotation and translation to every active pose, so increasing their weights adds no information along that common mode.This six-degree-of-freedom gauge freedom is conventionally removed by anchoring one pose.Source: E. LiDAR Factor via Scan-to-Multiscan… · p. 8
  • The LiDAR analysis uses right (body) perturbations, with the SE(3) adjoint converting them to equivalent left (world) perturbations.The perturbation contains rotational and translational increments, in that order.Source: E. LiDAR Factor via Scan-to-Multiscan… · p. 8

1) Factor Definition and Linearization

The LiDAR factor registers voxel means from the current scan against overlapping keyframes using a distribution-to-distribution VGICP cost. It creates separate relative constraints for each keyframe and refreshes fused information between Gauss–Newton iterations.

1) Factor Definition and Linearization

  • Each voxel correspondence compares a current-scan voxel mean with a matched keyframe voxel mean transformed into the current scan frame, using fused voxel-covariance information.This distribution-to-distribution VGICP residual defines the scan-to-keyframe factor.Source: 1) Factor Definition and Linearization · p. 8
  • The current scan forms separate factors against its overlapping keyframes rather than registering to a single fused map.Expressing both means in the current frame selects the reference frame for the relative pose.Source: 1) Factor Definition and Linearization · p. 8
  • The fused information is held constant within each Gauss–Newton step and refreshed between iterations at the updated relative pose.The factor’s linearization yields Hessian blocks and a gradient.Source: 1) Factor Definition and Linearization · p. 9

2) Gauge Null Space and Rank of the LiDAR Hessian

Relative scan-to-multiscan LiDAR factors are invariant to a common rigid transform, creating a six-dimensional structural gauge; additional rank loss reflects scene geometry.

2) Gauge Null Space and Rank of the LiDAR Hessian

  • A common global rigid transform leaves every relative pose and LiDAR residual unchanged, so the assembled LiDAR information matrix vanishes on the six-dimensional gauge subspace.In Fig. 5(b), scan-to-multiscan factors couple the current scan to keyframes through off-diagonal blocks.Source: 2) Gauge Null Space and Rank of the… · p. 9, Figure 5 · p. 9
  • For a connected pose graph with full-rank per-factor Hessians, the LiDAR matrix has rank 6N − 6 and exactly six global gauge modes.Rank deficiency in a per-factor Hessian adds geometric null directions beyond the global gauge.Source: 2) Gauge Null Space and Rank of the… · p. 9
  • Structural gauge deficiency is always six-dimensional, whereas geometric deficiency depends on scene structure and weakens relative directions outside the gauge.Insufficient surface-normal diversity in corridors, tunnels, or flat-terrain aerial surveys can weaken the per-factor Hessians.Source: 2) Gauge Null Space and Rank of the… · p. 9

3) Implications for Tightly-Coupled Fusion

GLIO2 uses relative LiDAR constraints so GNSS supplies the global datum, while online optimization jointly updates active keyframes and sensor states. Its implementation combines GPU-parallel LiDAR linearization with fixed-lag smoothing and a coarse-to-fine bootstrap.

3) Implications for Tightly-Coupled Fusion

  • Relative LiDAR factors add no direct prior on the window’s common rigid motion, leaving the global datum to GNSS; geometric degeneracy weakens only relative directions.DD pseudorange constrains global position, gravity constrains roll and pitch, and GNSS-constrained motion through Doppler and yaw initialization makes heading observable.Source: 3) Implications for Tightly-Coupled… · pp. 9–10

4) GPU-Parallel Linearization

  • Each nonlinear update jointly changes active keyframe poses, while the GPU re-associates correspondences and accumulates LiDAR factor blocks and the CPU solves the sparse graph.Factor data are serialized so each linearization pass uses two CPU–GPU transfers, independent of the number of active factors.Source: 4) GPU-Parallel Linearization · p. 10

F. Active Keyframe Set

  • Active keyframes are selected by scan overlap and optimized as states; updated poses trigger correspondence re-association, while pruning stops new factors but retains existing graph factors.Keyframes are inserted when novel and pruned when overlap falls below threshold or the set exceeds its cap.Source: F. Active Keyframe Set · p. 10

G. Marginalization

  • States leaving the fixed-lag window are Schur-complemented into a frozen linearized prior, while the shared extrinsic and gravity variables remain active and relinearized.Keeping those variables active holds the local-world frame anchored to ECEF across GNSS dropouts.Source: G. Marginalization · p. 10

H. Initialization

  • Initialization levels roll and pitch from stationary accelerometer data, obtains an ECEF anchor from GNSS, then estimates yaw by aligning a short LIO trajectory to GNSS positions.The initial gravity, anchor, and heading are approximate and continue to be refined online.Source: H. Initialization · pp. 10–11

I. Global Batch Optimization

The batch back-end addresses global bias left when an unobserved interval outlasts the fixed-lag window by jointly re-optimizing the whole trajectory. It reuses compact factors from online registration to avoid repeating dense LiDAR correspondence search.

I. Global Batch Optimization

  • When GNSS denial, NLOS, or stationarity leaves the global frame unobserved longer than the window, marginalized states can freeze an unconstrained global bias that later windows cannot revisit.The batch re-optimizes every frame jointly rather than using only a keyframe subsample.Source: §I · p. 11
  • The batch spans the full-session IMU, scan-to-multiscan LiDAR, and GNSS factors, replaces the marginalization prior with weak priors on the first state and shared extrinsic, and robustly down-weights inconsistent DD pseudorange observations.The DD-pseudorange robust kernel tightens the inlier set over successive rounds; the problem is solved by Levenberg–Marquardt.Source: §I · p. 11, Figure 6 · p. 11
  • The back-end retains marginalized relative-pose LiDAR factors with their online information matrices plus IMU and GNSS factors, discarding raw clouds so batch iterations avoid per-point search and linearization.Common rigid corrections preserve relative poses and correspondences, so they do not require LiDAR re-registration.Source: §I · pp. 11–12

V. Experiments

The experiments evaluate GLIO2 on public benchmarks and self-collected UAV and vehicle sequences chosen to span GNSS degradation and LiDAR degeneracy. The described setup includes onboard UAV processing and long-range vehicle recordings processed offline.

V. Experiments

  • The evaluation spans three public datasets, four self-collected sequences, module ablations, and two real-world deployments.The self-collected data target GNSS denial or NLOS and LiDAR geometric degeneracy.Source: §V · p. 12

1) Public Datasets

  • UrbanNav covers dense Hong Kong canyons, MARS-LVIG includes texture-poor aerial scenes, and M3DGR tests degraded and GNSS-denied ground-robot scenarios, including a single-receiver evaluation.The selected M3DGR sequences include open-sky and denied-region runs; its lack of a reference-station stream tests base-station-free generalization.Source: 1) Public Datasets · p. 12

2) Sensor Setup

  • The Switch and Plain UAVs run GLIO2 onboard on Jetson Orin NX, whereas the passenger-car recordings for Urban and Bridge are processed offline.The UAVs use identical sensor suites; the vehicle collection targets large-scale recording rather than onboard deployment.Source: 2) Sensor Setup · p. 12, Figure 7 · p. 12

3) Self-Collected Datasets

  • The four self-collected sequences isolate complementary conditions: Urban and Switch stress GNSS, while Bridge and Plain stress LiDAR geometry.Bridge is a 5.66-km vehicle run at up to 96 km/h; Plain’s flat ground leaves horizontal pose under-determined. The per-scan severity grading is illustrative, not a calibrated test.Source: 3) Self-Collected Datasets · pp. 12–13, Figure 8 · p. 13

4) Evaluation Metrics

GLIO2 completes all eleven public and self-collected sequences, with batch optimization further refining the global trajectory. Results span public benchmarks and self-collected routes that challenge GNSS reception and LiDAR geometry.

4) Evaluation Metrics

  • ATE is reported as horizontal 2D and full 3D RMSE; GNSS-aware systems are compared without trajectory alignment, while FAST-LIO2 is aligned over the first 33%.Switch’s indoor segment is assessed qualitatively through map continuity.Source: 4) Evaluation Metrics · p. 13

B. Experimental Results

  • GLIO2 processes all eleven public and self-collected sequences, and its batch stage further refines the global trajectory.The evaluations include GNSS NLOS and weak LiDAR geometry.Source: B. Experimental Results · p. 14

1) Public Dataset Benchmark Experiments

2) Self-Collected Dataset Benchmark Experiments

C. Ablation Studies

The ablations compare raw GNSS coupling, GPU acceleration, and scan-to-multiscan registration against alternatives. The runtime evaluation tests feasibility across LiDAR types and platforms.

1) Evaluation of Tightly-Coupled GNSS

2) Evaluation of GPU Acceleration

  • Moving LiDAR linearization to the CPU raises 3D RMSE to 2.61 m on Switch and 4.06 m on Urban; GPU processing averages 7.43 ms and 20.41 ms per scan.Dropped CPU scans lose constraints, while GPU processing retains accuracy within the scan budget.Source: Table 6 · p. 17, 2) Evaluation of GPU Acceleration · p. 17

3) Evaluation of Scan-to-Multiscan

1) Front-End Runtime

  • On Jetson Orin NX, GLIO2’s per-scan totals are 39.60 ms, 54.02 ms, and 55.54 ms across the three LiDARs, each below the 100 ms budget.The CPU-side iSAM2 update takes 28–35 ms, or 58–71% of the total.Source: Table 8 · p. 18, 1) Front-End Runtime · p. 18

2) Back-End Runtime

GLIO2’s offline back-end refines the full trajectory quickly, while onboard deployments test localization during facade cleaning and assistive navigation. The applications also expose a task-completion limitation under cleaning-spray interference, and the conclusion notes remaining GNSS initialization and quality-management constraints.

2) Back-End Runtime

  • 24.2 s versus 280 s on Whampoa, 7.1 s versus 110 s on Tst, and 1.3 s versus 25.9 s on Urban; GLIO2’s batch graph had about 3.3× as many nodes as GLIO’s.On the 30-min Whampoa sequence, the graphs contained 15,346 versus 4,603 nodes, respectively, and GLIO2 attained lower 3D RMSE.Source: 2) Back-End Runtime · p. 19

VI. Applications

  • GLIO2 served as the sole onboard localization and mapping source in a facade-cleaning UAV and a guide-dog quadruped, testing robustness, global consistency, and onboard real-time operation.The two applications were designed to stress simultaneous sensor degradation, long map-referenced routes, and fully onboard operation.Source: §VI · p. 19

A. Autonomous Facade-Cleaning UAV

  • 8 of 10 facade-cleaning missions finished autonomously; both takeovers followed cleaning-water spray being misclassified as obstacles, while localization remained stable for manual recovery.The UAV operated at a 1–2.5 m facade standoff, where LiDAR degeneracy and limited GNSS satellite visibility occurred together.Source: A. Autonomous Facade-Cleaning UAV · pp. 19–20

B. Guide-Dog Navigation for the Visually Impaired

  • All five repeated 210 m guide-dog route walks reached the intended destination without manual intervention, including through the GNSS-denied segment.The route included a LiDAR-degenerate stretch followed by a GNSS-denied stretch.Source: B. Guide-Dog Navigation for the… · p. 20

VII. Conclusion and Future Work

  • GLIO2 combines dense scan-to-multiscan LiDAR, IMU pre-integration, and raw double-differenced pseudorange and Doppler in a sliding-window graph; keeping factors re-associable and relinearizable supports the reported robustness.The conclusion also describes an efficient offline back-end and a single-receiver variant for base-station-free settings.Source: §VII · p. 20
  • 1.6 m horizontal accuracy online on the 5.66-km bridge, where evaluated LiDAR-based baselines diverged; GLIO2 (Batch) was most accurate on Whampoa at 1.96 m horizontal and 2.99 m 3D.The full pipeline averaged 39.60–55.54 ms per scan on a Jetson Orin NX, and the offline back-end re-optimized a 30-min session in about 24 s.Source: §VII · p. 20
  • Both GNSS-LIO configurations depend on adequate GNSS at startup; poor initial GNSS conditions or initialization during dynamic motion degrades the fused estimate.The current GNSS quality-management treatment is limited to elevation/SNR weighting and residual-based fault detection and de-weighting.Source: §VII · p. 21

Appendix A GNSS Factor Jacobians

The appendix details GNSS residual Jacobians, including Doppler sensitivities linked to antenna lever-arm motion. For base-station-free datasets, it replaces double differencing with a single-receiver pseudorange factor and reinstates receiver clock offset in the state.

Appendix A GNSS Factor Jacobians

  • Base-station range terms contribute no state Jacobian because the base coordinates are fixed; the geometric-range derivative uses the rover line-of-sight vector.The appendix states that the relevant Jacobians use the unit line-of-sight vector pointing from receiver to satellite.Source: Appendix A · p. 23
  • The dominant Doppler rotation coupling is the velocity-scale term in the orientation Jacobian, arising from tangential lever-arm velocity; related small terms are retained for consistency with pseudorange Jacobians.The orientation and gyro-bias Jacobians both include the tangential lever-arm velocity contribution.Source: Appendix A · p. 23

Appendix B Single-Receiver Pseudorange Factor

  • For datasets without a reference station, the single-receiver factor reinstates receiver clock offset as a state variable, and its short M3DGR fused estimate remains sub-meter despite broadcast-corrected SPP accuracy.The single-receiver variant retains atmospheric and orbit/clock errors that double differencing would otherwise remove.Source: Appendix B · p. 23

Related papers

Loading 2610.12411v1…