G-EDF-Loc: 3D Continuous Gaussian Distance Field for Robust Gradient-Based 6DoF Localization

G-EDF-Loc: Continuous Gaussian Distance Fields for Robust Gradient-Based 6DoF Localization

Introduction

G-EDF-Loc addresses core limitations in current 6DoF localization systems for mobile robotics by introducing a CPU-efficient, memory-compact, and analytically differentiable 3D distance field representation. This is achieved by explicitly regressing the Euclidean Distance Field (EDF) of the map using a Block-Sparse Gaussian Mixture Model (GMM) with adaptive spatial partitioning. Distinct from the prevailing discretized or neural implicit surface models, the approach ensures strict $C^1$ continuity (i.e., continuous gradients) across block transitions, enabling robust and scalable gradient-based scan-to-map registration even in the absence of reliable odometry or IMU priors.

Block-Sparse Gaussian EDF: Theory and Construction

The core G-EDF construct models the EDF as a weighted sum of adaptively initialized anisotropic, axis-aligned Gaussian kernels over sparse 3D spatial blocks. Unlike traditional GMMs used for density estimation, the weights here are not normalized and can be negative, enabling the model to tightly fit sharp surface features by constructive and subtractive interference. To avoid local misalignments or capacity overuse due to fixed kernel counts, an adaptive strategy is employed: initialization utilizes Non-Maximum Suppression on the local EDT, seeding positive kernels at local maxima and negative at local minima, providing a geometrically meaningful warm start before iterative nonlinear least squares fitting.

The system maintains high fidelity while achieving remarkable compression, adapting the kernel density to local geometric complexity. Local ground-truth distances are computed on high-resolution grids via EDTs, and fitting is performed per block. Overlapping bands between blocks are explicitly blended with Smoothstep cubic Hermite polynomials, ensuring strict global $C^1$ continuity and eliminating the gradient discontinuities that can induce optimizer failures in classical grid-based fields.

This structure allows memory-efficient, parallel construction, as illustrated with reconstructions over large-scale environments such as the Snail dataset.

Figure 1: Detailed perspectives and top-down extent of the Snail dataset environment reconstructed using G-EDF.

Analytical Gradients and Optimization Consistency

A closed-form analytical gradient for each query point is possible due to the Gaussian functional form, obviating the need for numerical differentiation or GPU acceleration. Empirical evaluations verify the Eikonal property ($\|\nabla d\| \approx 1$) almost everywhere in the field, ensuring optimizer stability and high convergence rates—a critical property for scan-to-map registration.

The block-overlapping blending approach is crucial: without blending, EDF and its gradient exhibit visible discontinuities and spurious local minima (Figure 2). The global blending ensures that not only the field but its spatial derivatives are continuous (Figure 2, right column).

Figure 2: Cross-section of the EDF and its gradient in a representative scene, contrasting the effect of blending (ensuring $C^1$ continuity) versus discrete blocks.

Direct Gradient-Based 6DoF Localization Pipeline

The proposed scan-to-map alignment pipeline is grounded on two main components: an optional Error-State Kalman Filter (ESKF) for prior motion propagation (when IMU data is available), and a robust analytic gradient-based registration module that minimizes the squared implicit surface distance of transform-corrected scan points.

Instead of relying on explicit correspondences (ICP variants) or feature extraction (LOAM, NDT), the system directly optimizes the rigid transform to minimize the aggregate field distance over the scan. Outlier rejection is performed dynamically, and a two-stage coarse-to-fine nonlinear least-squares protocol ensures robust convergence under severe initial misalignment or in the presence of dynamic scene clutter.

Even in the complete absence of IMU data or good priors, G-EDF-Loc reliably converges due to the smooth, artifact-free distance landscape and the system's ability to handle large spatial misalignments.

Empirical Evaluation and Results

Extensive benchmarks were performed on large-scale, heterogeneous public datasets (New College, Snail). The G-EDF representation achieves mean absolute errors $(\sim3\,\mathrm{cm})$ with median errors below $2\,\mathrm{cm}$ and gradient norms very close to ideal Eikonal behavior, both for moderate ($315\times260\times40\,\mathrm{m}^3$) and very large-scale ($900\times605\times120\,\mathrm{m}^3$) environments.

For localization, the pipeline is compared side-by-side with multi-threaded state-of-the-art Fast-GICP and NDT from the widely adopted hdl_localization library. Under nominal conditions (good IMU/inertial priors and standard odometry), all three achieve comparable cm-level translation and sub-$2^\circ$ rotation RMSEs. However, under progressively degraded conditions—no IMU, increasing Gaussian noise in priors—NDT and Fast-GICP experience severe performance degradations, including temporal and spatial instability or outright divergence, with average per-scan latency increasing to and exceeding $1\,\text{s}$ in challenging cases.

In contrast, G-EDF-Loc remains robust: errors barely increase and compute times remain moderate (median $C^1$0 per scan), never diverging. Even when baselines fail to converge, as in aggressive-motion or poor prior scenarios, G-EDF-Loc continues to produce consistent, accurate trajectories:

Figure 3: Example trajectory estimates for the “bc” Snail sequence. G-EDF-Loc maintains alignment under inertial, no-IMU, low-noise, and high-noise prior setups while GICP and NDT diverge or experience jumps.

Implications and Future Directions

This work demonstrates that adaptive, block-sparse, analytically differentiable field models based on Gaussian mixtures are effective for large-scale robot localization under strong computational constraints. The method realizes three essential theoretical advantages:

• Globally consistent, memory compact, $C^1$1-continuous EDFs,
• Analytical gradients for robust, fast optimizer convergence,
• CPU-only, scalable inference suited for embedded and real-time deployment.

Strong claims supported by quantitative benchmarks: G-EDF-Loc matches or surpasses discrete grid and point cloud registration baselines in both accuracy and computational stability, especially as odometric priors degrade. The approach's resilience to severe odometry loss (including total IMU failure) surpasses competing methodologies.

Future research should explore multi-resolution representations for even faster wide-basin convergence and extend the block-sparse formulation toward online mapping and closed-loop SLAM on embedded hardware. Further, the connection with neural implicit representations remains ripe for exploration—fusion of learned priors with the current analytic model could yield improved extrapolation and generalization in unseen environments.

Conclusion

G-EDF-Loc offers a principled, efficient framework for high-fidelity 3D distance field representation and 6DoF localization using scan-to-map registration, eliminating critical smoothness and scalability bottlenecks present in discrete and implicit approaches. Its robust performance across challenging and large-scale settings positions it as a compelling alternative for real-time localization in autonomous systems, with substantial practical and theoretical implications for map representation and robot navigation.