Real-Time Learning of Signed Distance Function via Kernel Regression with Uncertainty Quantification

Zhirui Dai1, Tianxing Fan1, Mani Amani1, Jaemin Seo2, Ki Myung Brian Lee1,
Ki Myung Brian Lee1, Hyondong Oh2, Nikolay Atanasov1
1Existential Robotics Lab, University of California San Diego
2 Department of Mechanical Engineering, Korea Advanced Institute of Science and Technology (KAIST)
Paper Code

An example of 3D SDF mapping using the proposed Kernel-SDF method. Left: Visualization of the learned SDF as a heatmap, where warmer colors indicate higher SDF values (free space) and cooler colors indicate lower SDF values (occupied space), as well as the voxels (colored by height) that indicate the locations where local BHMs and GPs are instantiated. Right: The online mesh reconstructed in real-time from the surface estimation front-end of Kernel-SDF.

Abstract: Accurate and efficient environment representation is crucial for robotic applications such as navigation, path planning, and manipulation. Signed distance function (SDF) has emerged as a powerful representation due to its ability to encode obstacle boundaries and provide distance information, useful for collision checking in motion planning and safety constraint specification in autonomous navigation. However, existing SDF learning methods face significant limitations: voxel-based approaches suffer from fixed resolution constraint and lack uncertainty quantification, neural network methods require high computational cost for training, and existing Gaussian process (GP) based methods struggle with scalability, sign estimation, and inconsistent uncertainty quantification. In this letter, we propose Kernel-SDF, which uses kernel regression to learn SDF with uncertainty quantification in real-time. Our approach consists of a surface estimation front-end that handles sensor noise and dynamic environment changes by using kernel regression in Hilbert space to learn the surface as a continuous occupancy field, and a back-end that learns accurate SDF using GPs, i.e., kernel regression in the functional space. Our method provides accurate SDF estimates, gradient predictions, uncertainty quantification, and online mesh construction at real-time rates. Evaluation results show that Kernel-SDF achieves superior accuracy compared to existing methods and outstanding real-time performance, making it suitable for various robotic applications requiring reliable environment representation with uncertainty awareness.

Methodoogy

Overview

Method Overview
Our method consists of a surface estimation front-end and an SDF prediction back-end. The front-end estimates surface points and normals from sensor observations using Bayesian Hilbert Map (BHM) and marching squares/cubes algorithm. The estimated surface points are then used as training data for the GPs in the back-end. The back-end learns the SDF using multiple GPs, which predict the SDF and its gradient with uncertainty quantification computed via softmin-based fusion.

SDF Prediction Back-End

GP SDF Back-End

The SDF prediction back-end uses multiple GPs to learn the SDF from the surface points estimated by the front-end. Each GP is associated with a voxel in a hierarchical tree that partitions the space.

Given a set of surface points $\mathcal{D}_\text{surf}=\{ \mathbf{x}_i, \sigma^2_i \}_{i=1}^{N}$, where $\sigma^2_i$ is the uncertainty of the surface point $\mathbf{x}_i$ estimated by the front-end, we can train the corresponding GP for predicting the shortest distance $|d(\mathbf{x})|$ by regressing a surrogate function $f$, which should be monotonically decreasing with respect to the distance $|d(\mathbf{x})|$ to the nearest surface point, such that $f\rightarrow 0$ as $|d(\mathbf{x})| \rightarrow \infty$ and $f \rightarrow 1$ as $|d(\mathbf{x})|\rightarrow 0$, which is consistent with zero-mean prior GP regression that as the query point goes far away from the training set, the posterior mean goes to zero. With the trained GP, we can predict the unsigned distance $|d(\mathbf{x}_\ast)|=r(\hat{f})$, where $r(\cdot)$ is the inverse function of the surrogate function $f(\cdot)$. The sign of the SDF can be determined by querying the occupancy probability from the front-end BHM at the query point $\mathbf{x}_\ast$.

Surface Estimation Front-End

The role of the surface estimation front-end is to provide reliable estimates of on-surface points, their uncertainty, and optionally the corresponding normals. We use the Bayesian Hilbert map (BHM) for this purpose, although other front-ends may also be used.

Given a dataset $\mathcal{D}_\text{BHM}=\{ \mathbf{x}_k, y_k \}_{k=1}^{K}$ is generated from the sensor measurement $\mathcal{S}_t=(\mathbf{R}_t, \mathbf{o}_t, \mathcal{P}_t)$ by sampling occupied samples as $(\mathbf{R}_t\mathbf{p}_i+\mathbf{o}_t, 1)$ and free-space samples as $(\lambda \mathbf{R}_t \mathbf{p}_i + \mathbf{o}_t, 0)$ for $\lambda \sim \mathcal{U}(0, 1)$ and $\forall \mathbf{p}_i \in \mathcal{P}_t$. We can update the BHM with an Expectation-Maximization algorithm, which enables incremental updates of the BHM. Hence, the BHMs can filter out sensor noise by combining multiple noisy measurements over time and adapt to dynamic changes in the environment.

BHM

High-quality Mesh Reconstruction

Replica Dataset with Simulated Depth Sensor Noise

Ground Truth

Ground Truth

Ours

Ours

FIESTA

FIESTA

Voxblox

Voxblox

iSDF

iSDF

VDB-GPDF

VDB-GPDF

The Cow in Cow and Lady Dataset

Ground Truth

Ground Truth

Ours

Ours

FIESTA

FIESTA

Voxblox

Voxblox

iSDF

iSDF

VDB-GPDF

VDB-GPDF

Newer College Dataset

Ground Truth

Ground Truth

Ours

Ours

FIESTA

FIESTA

Voxblox

Voxblox

iSDF

iSDF

VDB-GPDF

VDB-GPDF

Robust to Sensor Noise

Mesh Quality vs. Sensor Noise
SDF Accuracy vs. Sensor Noise

SDF Prediction with Consistent Uncertainty Quantification

SDF Prediction Slice

SDF Prediction

SDF Error Slice

SDF Error

SDF Uncertainty Slice

SDF Uncertainty

Acknowledgement

We gratefully acknowledge support from ARL DCIST CRA and the Ministry of Trade, Industry and Energy (MOTIE), Korea, under the Strategic Technology Development Program, supervised by the Korea Institute for Advancement of Technology (KIAT) [Grant No. P0026052].