PaperlayerView exact revision on arXiv

Source-linked AI summary

Unsigned Distance Maps on 2D Point Cloud Registration

Ricardo B. Sousa, Giorgio Grisetti, Héber Miguel Sobreira, Carlos André Silva, António Paulo Moreira

arXiv:2609.25932v1cs.RO

TL;DR

2D registration methods either repeat nearest-neighbor searches during ICP or use correspondence-free approaches oriented toward scan-to-map alignment, leaving open a unified representation for point-to-point and point-to-plane residuals. The paper uses unsigned distance maps to formulate both residual types on SE(2), and reports that its precomputed point-to-point variant outperforms analytical counterparts on a synthetic benchmark and the IILABS 3D dataset while achieving competitive laser-odometry drift versus point-to-plane formulations.

  • Problem

    The paper addresses the gap between ICP’s repeated nearest-neighbor association and scan-to-map-oriented correspondence-free methods by seeking a unified representation for point-to-point and point-to-plane residuals.

    Source: §I · p. 1

  • Method

    The approach uses unsigned distance maps to formulate point-to-point and point-to-plane residuals on SE(2), replacing iterative nearest-neighbor searches with distance-field queries.

    Source: §I · p. 1

  • Results

    On the synthetic benchmark and IILABS 3D dataset, Dp2p(M1) consistently outperformed analytical point-to-point counterparts and reached competitive laser-odometry drift against point-to-plane formulations.

    Source: §VI · p. 8

  • Takeaways & Limitations

    The finite-difference gradient makes Dp2p(M1) act as an implicit point-to-plane constraint in planar regions while regularizing sensor noise.

    Source: §VI · p. 8

Abstract

from arXiv · show

2D point cloud registration arises in laser odometry and Simultaneous Localization and Mapping (SLAM) for mobile robots. Iterative Closest Point (ICP) is one of the most widely used approaches. Still, its iterative procedure recomputes correspondences via nearest-neighbor search at every iteration, whereas correspondence-free alternatives focus on scan-to-map alignment. This paper proposes a 2D point cloud registration approach based on unsigned distance maps, precomputing the Euclidean distance to the nearest reference point, along with its spatial derivatives, over a discrete grid, replacing the per-iteration search with O(1) lookups. Moreover, point-to-point and point-to-plane error formulations are derived on the SE(2) manifold and solved via Gauss-Newton optimization. On a synthetic benchmark and the real-world IILABS 3D dataset, the precomputed point-to-point variant outperforms its analytical counterparts, achieving competitive laser-odometry drift compared to point-to-plane formulations, as the precomputed gradient regularizes correspondences in the presence of sensor noise.

I. INTRODUCTION

The paper proposes unsigned distance maps for 2D point-cloud registration, replacing per-iteration nearest-neighbor association with O(1) distance-field queries and supporting point-to-point and point-to-plane optimization on SE(2). It evaluates the approach on synthetic and real-world data, reporting that the precomputed point-to-point variant outperforms analytical counterparts and achieves competitive odometric drift.

I. INTRODUCTION

  • ICP, repeatedly establishes correspondences by nearest-neighbor search, while prior correspondence-free methods target scan-to-map alignment or rely on specific assumptions.The introduction identifies a need for a unified representation supporting both residual types without per-iteration nearest-neighbor search.Source: §I · p. 1
  • Unsigned distance maps replace per-iteration nearest-neighbor searches with O(1) distance-field queries for moving points.The approach derives point-to-point and point-to-plane formulations on the SE(2) manifold and solves them with Gauss-Newton optimization.Source: §I · p. 1
  • The framework offers three representation levels, dense or sparse storage, and a drop-in replacement for kd-tree data association in classical ICP,.It is presented as a unified open-source framework for 2D point-cloud registration.Source: §I · p. 1
  • The proposed distance-field point-to-point and point-to-plane residuals include SE(2) Jacobians; the analytical point-to-plane formulation is equivalent to classical point-to-plane ICP.Source: §I · p. 1
  • The precomputed point-to-point variant outperforms analytical counterparts and achieves competitive odometric drift versus point-to-plane formulations on synthetic and IILABS 3D data.The authors attribute its robustness to sensor noise to regularization from the precomputed finite-difference gradient.Source: §I · p. 1

II. RELATED WORK

Prior work spans correspondence-based ICP, probabilistic and map-based alignment, and registration using precomputed spatial gradients. Distance maps provide correspondence-free error lookups for scan-to-scan and scan-to-map alignment, motivating their adaptation beyond PerfectMatch,.

II. RELATED WORK

  • ICP alternates nearest-neighbor correspondence search with transformation estimation, while point-to-plane ICP outperformed point-to-point ICP in a comparison by Rusinkiewicz and Levoy.Chen and Medioni use target-surface normals to measure the squared projected residual.Source: §II · p. 2
  • NDT aligns scans through Gaussian cell distributions without explicit correspondences; 3D-NDT reduced computation cost versus point-to-point ICP with kd-tree search.GICP retains iterative kd-tree association but weights residuals by local surface covariance, while NICP adds line-of-sight association and 6D point-normal Mahalanobis optimization, outperforming GICP and NDT in accuracy.Source: §II · p. 2
  • Probabilistic 2D methods include Mahalanobis-based correspondence alignment and GHT-generated alignment hypotheses, while GMapping and Cartographer align scans to maps or submaps.Montesano et al. report faster convergence than ICP; GMapping uses grid-map observation likelihood, and Cartographer optimizes smoothed occupancy values.Source: §II · p. 2
  • HectorSLAM uses bilinearly interpolated occupancy probabilities and approximate derivatives, while PerfectMatch, uses distance maps and precomputed gradients for scan-to-map alignment.Sobreira et al. found PerfectMatch faster than ICP and NDT and more tolerant to orientation initialization errors.Source: §II · p. 2
  • Distance maps evaluate source-point errors through precomputed lookups without explicit association, supporting scan-to-scan and scan-to-map alignment; adapting PerfectMatch, may enable scan-to-scan registration.The related-work discussion contrasts this with ICP’s per-iteration correspondence steps and probabilistic approaches that typically require a map estimate over time.Source: §II · p. 2

III. UNSIGNED DISTANCE MAPS · A. Spatial derivatives

The method represents each query by its unsigned distance to the nearest reference point, then precomputes distance and derivative lookups on a grid. Its spatial derivatives are obtained from the distance field analytically or estimated by finite differences on the discrete grid.

III. UNSIGNED DISTANCE MAPS

  • The distance map is defined for a reference target point cloud acquired by a planar laser scanner and expressed in the sensor frame.Source: §III · p. 2
  • The map assigns each sensor-frame query its Euclidean distance to the nearest reference point, so D is nonnegative and unsigned.Unsigned distance fields are continuous everywhere but not everywhere differentiable.Source: §III · p. 2
  • Precomputing the map over a grid lets each transformed moving point retrieve distance and derivatives in O(1), without recomputing correspondences against the reference cloud.Each cell implicitly stores nearest-reference-point data association.Source: §III · p. 2

A. Spatial derivatives

  • Where the nearest reference point is unique, D is smooth and its gradient is the unit vector from that point to the query; the gradient is undefined at equidistant ties.The gradient has unit norm almost everywhere, the Eikonal property of Euclidean distance fields.Source: A. Spatial derivatives · p. 2
  • On the discrete grid, the gradient is estimated by central finite differences, with one-sided differences at grid borders.The Hessian is computed as the centered difference of normalized gradients.Source: A. Spatial derivatives · p. 3
  • The finite-difference Hessian may be nonsymmetric, unlike the analytical expression, so the representation stores all four Hessian entries.Source: A. Spatial derivatives · p. 3

B. Representation levels · C. Data structures

The paper organizes distance maps into three representation levels that trade memory for precomputed information, and implements them with dense or sparse data structures supporting O(1) lookup. The maps are built by wavefront propagation, with gradients and Hessians computed in post-processing.

B. Representation levels

  • Three representation levels provide increasing amounts of precomputed information and memory cost while keeping queries at O(1).Source: B. Representation levels · p. 3
  • M0 stores only each cell’s nearest-reference-point index, recovering distance, gradient, and Hessian analytically on demand as an O(1) replacement for kd-tree search.It is the most memory-efficient layout and reduces per-query cost from O(log n) to O(1).Source: B. Representation levels · p. 3
  • M1 stores distance and gradient components, optionally the nearest-point index, and estimates any needed Hessian by centered differences at query time.Source: B. Representation levels · p. 3
  • M2 additionally stores every finite-difference Hessian entry, making second-order information available at O(1).Source: B. Representation levels · p. 3

C. Data structures

  • Map construction uses best-first wavefront propagation over an 8-connected neighborhood, seeding reference-point cells at zero and propagating nearest-point indices until the map or truncation distance dmax is reached [20].Post-processing passes compute gradients and Hessians by finite differences.Source: C. Data structures · p. 3
  • Nano (M0,dense) uses a dense integer-index array with the lowest memory footprint, while Standard (M1|2,dense) uses a dense array of full cells.Source: C. Data structures · p. 3
  • Sparse (M1|2,sparse) stores only cells within D ≤dmax in a hash table, providing average O(1) lookup and mitigating hash-collision worst cases through rehashing.Source: C. Data structures · p. 3

IV. REGISTRATION METHODOLOGY · A. Problem formulation

The methodology formulates point-to-point and point-to-plane registration constraints on SE(2), using unsigned-distance-map lookups and manifold Gauss–Newton optimization. It defines the moving-cloud alignment problem and the right-sided pose update used for optimization.

IV. REGISTRATION METHODOLOGY

  • Table I compares the nano, standard, and sparse distance-map variants.Source: Table I · p. 3
  • Hash lookup averages O(1) but can reach O(Nd) under collisions, mitigated by post-propagation rehash, with Nd ≤ NG.Nd is the number of populated cells within dmax, and NG is the total number of cells.Source: §IV · p. 3
  • The point-to-point and point-to-plane factors replace ICP’s per-iteration nearest-neighbor search with O(1) lookups of the unsigned distance map and its derivatives.They are implemented in the srrg2 solver framework with its manifold Gauss–Newton optimizer.Source: §IV · p. 3

A. Problem formulation

  • Registration seeks the SE(2) robot pose that best aligns the moving cloud, whose points are expressed in the sensor frame, to the reference cloud.Source: A. Problem formulation · p. 3
  • The alignment is a nonlinear least-squares problem solved by Gauss–Newton, with tangent-space increments applied through the right-sided update xₖ₊₁ = xₖ · exp(Δx).The perturbation is Δx = (Δx, Δy, Δθ)ᵀ ∈ R³; the formulation also uses a 2D skew operator to encode rotational sensitivity in the Jacobians.Source: A. Problem formulation · p. 3

B. Point-to-point error formulation

The point-to-point formulation uses the distance map’s value at each projected moving point as a scalar residual measuring its distance to the nearest reference point. Its Jacobian combines the distance-map gradient with the classical point-to-point ICP Jacobian.

B. Point-to-point error formulation

  • The scalar residual is the distance-map value at the projected moving point, measuring its Euclidean distance to the nearest reference point.The nearest-reference distance is encoded by the precomputed field D.Source: B. Point-to-point error formulation · p. 3
  • The Jacobian multiplies the distance-map gradient by the classical point-to-point ICP Jacobian, with the gradient retrieved in O(1) from M1 or M2 or evaluated analytically at M0.The Jacobian is obtained by applying a right-sided perturbation and differentiating through the chain rule at Δx = 0.Source: B. Point-to-point error formulation · p. 4

C. Point-to-plane error formulation · D. Surface-normal estimation

The point-to-plane formulation uses moving-scan surface normals to project the direction toward the nearest reference point, while normals are estimated once per scan before optimization.

C. Point-to-plane error formulation

  • The residual projects the rotated moving normal onto the nearest-reference displacement direction, represented by the normalized distance-map gradient times distance, following Chen and Medioni.This incorporates surface-normal information to improve convergence in environments dominated by planar structures.Source: C. Point-to-plane error formulation · p. 4
  • The 1 × 3 Jacobian includes normal rotation, gradient-direction change through the distance-map Hessian, and distance-value change; the latter two share the point-to-point ICP Jacobian factor.Differentiating the residual with respect to ∆x yields these three terms.Source: C. Point-to-plane error formulation · p. 4
  • At M0, the residual equals the classical point-to-plane ICP residual, making the formulations analytically identical, as numerically confirmed in Section V.With M2, the gradient and distance needed for the additional Jacobian terms are retrieved in O(1).Source: C. Point-to-plane error formulation · p. 4

D. Surface-normal estimation

  • Normals are estimated once per scan using angularly ordered neighborhoods collected in both scan directions until points exceed radius r, avoiding a kd-tree search.The traversal wraps at scan boundaries and yields a spatially coherent neighborhood.Source: D. Surface-normal estimation · p. 4
  • With at least Nmin ≥ 3 neighbors, PCA on the 2 × 2 covariance selects the least-variance eigenvector as the normal, which is oriented toward the sensor origin.This orientation makes obstacle-boundary normals face the scanner.Source: D. Surface-normal estimation · p. 4
  • Points with fewer than Nmin ≥ 3 neighbors are marked invalid for the point-to-plane term.The minimum-neighbor threshold determines whether PCA-based normal estimation is performed.Source: D. Surface-normal estimation · p. 4

V. EXPERIMENTAL RESULTS

The evaluation compares distance-map registration formulations with ICP baselines in a controlled synthetic benchmark and a real-world laser-odometry experiment.

V. EXPERIMENTAL RESULTS

  • The synthetic benchmark measures pairwise registration accuracy and uncertainty estimation against known ground-truth poses, while the real-world evaluation assesses odometric drift over extended trajectories.Both experiments compare Dp2p and Dp2pl with distance-map ICP point-to-point and point-to-plane baselines.Source: §V · p. 4
  • All methods use a common SE(2) Gauss–Newton optimizer in srrg2, with Dp2p tested at M0/M1, Dp2pl at M0/M2, and both ICP baselines at M0.Source: §V · p. 4

A. Synthetic benchmark

In the synthetic benchmark, point-to-plane formulations achieve lower or equal registration errors than point-to-point formulations, while analytical point-to-plane is most accurate. Precomputed variants trade some accuracy for O(1) lookup, and analytical point-to-plane formulations provide the best-calibrated uncertainty estimates.

a) Experimental setup

  • The benchmark uses 100 trials per circle, square, and corridor scene, and reports RMSE for observable pose-error components and mean χ²ε across noise-free and noisy scans at two grid resolutions.The scenes test rotational degeneracy, pose constraints up to symmetry, and unobservable translation along the corridor axis.Source: A. Synthetic benchmark · pp. 4–5

c) Registration accuracy

  • Point-to-plane variants achieve lower or equal translation and rotation error than point-to-point variants, while analytical Dp2pl(M0) is most accurate overall.The comparison extends to distance-map formulations; the analytical point-to-plane result is contrasted with accuracy losses from precomputation.Source: Table II · p. 5, Table III · p. 5, A. Synthetic benchmark · pp. 5–6
  • At δ = 0.05 m with noise and no voxelization, interpolation raised εt from 0.003 to 0.039 m (circle), 0.004 to 0.047 m (square), and 0.008 to 0.036 m (corridor).The passage attributes the degradation to zero-initialized hit cells biasing the interpolated distance field.Source: A. Synthetic benchmark · p. 6
  • Precomputed methods retain O(1) lookup but lose some analytical accuracy: Dp2pl(M2) beats point-to-point formulations yet falls short of analytical Dp2pl(M0) and ICPD, p2pl.Grid quantization and finite-difference gradients bias residual directions; M2's second-order derivative compounds finite differences and is more noise-sensitive.Source: A. Synthetic benchmark · p. 6
  • On the noise-free square, Dp2pl(M0) and ICPD, p2pl reach χ²ε ≈ 0; on the circle, ICPD, p2p reaches ≈ 6.6×10^3 versus point-to-plane ≈ 46–190.Analytical point-to-point formulations are overconfident on degenerate scenes.Source: A. Synthetic benchmark · p. 6
  • Dp2p(M1) has lower χ²ε than Dp2p(M0) and ICPD, p2p on circle and corridor scenes, whereas noisy-circle Dp2pl(M2) reaches ≈ 1120–1717, favoring M0 point-to-plane for uncertainty-aware use.The passages attribute M1's lower values to regularization from its finite-difference gradient and cell resolution, and M2's higher values to compounded second-order finite differences.Source: A. Synthetic benchmark · p. 6

B. Real-world laser odometry evaluation

On the IILABS 3D dataset, the precomputed point-to-point formulation Dp2p(M1) outperformed the ICP point-to-point baseline across most configurations and achieved competitive drift on three sequences. Drift also varied with the keyframe-splitting criterion.

Experimental setup and evaluation metrics

  • Evaluation used Hokuyo UST-10LX scans from four IILABS 3D planar sequences, OptiTrack ground truth, and mean relative drift over 10 m segments.The sequences were Nav A Diff, Nav A Omni, Loop, and Slippage; relative metrics were used to assess local front-end consistency independently of global alignment.Source: B. Real-world laser odometry evaluation · p. 6

c) Laser odometry drift

  • Dp2pl(M0) and ICPD, p2pl [2] were numerically equivalent in 34 of 48 configurations by RTE and 38 of 48 by RRE, while M0 point-to-plane usually beat point-to-point.The exception was the 90 % inlier threshold, where point-to-plane degraded on Nav A Diff, Nav A Omni, and Slippage for both distance-map and ICP variants.Source: B. Real-world laser odometry evaluation · p. 6
  • Dp2p(M1) beat ICPD, p2p [1] in 39 of 48 configurations on RTE and 35 of 48 on RRE, while achieving competitive drift against point-to-plane methods.Its best figures were 0.95 %/0.078 °m−1 on Nav A Diff, 0.81 %/0.063 °m−1 on Nav A Omni, and 0.55 %/0.056 °m−1 on Slippage. The finite-difference gradient acts as an implicit point-to-plane constraint in planar regions while regularizing sensor noise, without explicit normal estimation.Source: B. Real-world laser odometry evaluation · pp. 6, 8, Table IV · p. 7
  • On Loop, Dp2p(M1) retained a gap (2.14 % versus 1.37 % for ICPD, p2pl [2]); Dp2pl(M2) was competitive for translation splits but worse at 80 %–90 % inlier thresholds.Near doorway discontinuities, gradient directions deviate from surface normals; Dp2pl(M2)'s threshold performance mirrors its noise sensitivity in the synthetic benchmark.Source: B. Real-world laser odometry evaluation · p. 8

Keyframe-splitting criterion

  • Translation-based keyframe splitting performed best at 2.0–2.5 m, while inlier-ratio splitting performed best at 40 %–60 % and degraded beyond 80 %.In most configurations, the 40 % inlier threshold yielded lower or equal drift than the best translation threshold.Source: B. Real-world laser odometry evaluation · p. 8

VI. CONCLUSIONS

The paper proposes an unsigned-distance-map framework for 2D point-cloud registration, with precomputed O(1) grid lookups and SE(2) point-to-point and point-to-plane residuals. Its precomputed point-to-point variant outperformed analytical point-to-point counterparts and achieved competitive laser-odometry drift, while analytical point-to-plane methods remained best calibrated on degenerate scenes.

VI. CONCLUSIONS

  • On synthetic and real-world IILABS 3D, Dp2p(M1) consistently outperformed analytical point-to-point counterparts and achieved competitive laser-odometry drift against point-to-plane formulations.Its finite-difference gradient acts as an implicit point-to-plane constraint within planar regions while regularizing sensor noise.Source: §VI · p. 8
  • Analytical M0 point-to-plane methods were best calibrated on degenerate scenes and remain preferable for uncertainty-aware applications.The precomputed gradient in Dp2p(M1) nevertheless regularizes the point-to-point information-matrix estimate relative to analytical counterparts.Source: §VI · p. 8
  • The framework uses unsigned distance maps for O(1) grid lookups over a precomputed field and derives point-to-point and point-to-plane residuals on SE(2).Source: §VI · p. 8
  • Future work will compare computational performance with kd-tree -based ICP and integrate the framework into a full SLAM pipeline.Source: §VI · p. 8
Loading 2609.25932v1…