PaperlayerView exact revision on arXiv

Source-linked AI summary

Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry

Chongjian Yuan, Wei xu, Xiyuan Liu, Xiaoping Hong, Fu Zhang

arXiv:2109.07082v5cs.RO

TL;DR

LiDAR odometry needs maps that register scans efficiently while representing uncertainty from noisy measurements, changing point densities, and pose estimation. The paper introduces an adaptive voxel map with probabilistic plane features, coarse-to-fine octree-hash organization, and integration into an iterated extended Kalman filter. Experiments report high accuracy and efficiency on KITTI and adaptability across indoor and unstructured outdoor environments and LiDAR scanning patterns.

  • Problem

    LiDAR odometry requires an efficient map for scan registration, but point-cloud maps inadequately represent measurement uncertainty and varying point densities.

  • Method

    The method uses adaptive-size coarse-to-fine voxels containing probabilistic plane features, organized with octrees and hashing and integrated into an iterated extended Kalman filter.

  • Results

    The method achieves better performance than point-cloud-based, NDT-based, and surfel-based methods on KITTI and performs well across structured, indoor, and unstructured environments.

  • Takeaways & Limitations

    The voxel representation supports accurate and efficient online LiDAR odometry across different environments and LiDAR scanning patterns.

  • Takeaways & Limitations

    The current voxel map uses plane features; adding edge features is identified as a direction for expanding its universality.

Abstract

from arXiv · show

This paper proposes an efficient and probabilistic adaptive voxel mapping method for LiDAR odometry. The map is a collection of voxels; each contains one plane (or edge) feature that enables the probabilistic representation of the environment and accurate registration of a new LiDAR scan. We further analyze the need for coarse-to-fine voxel mapping and then use a novel voxel map organized by a Hash table and octrees to build and update the map efficiently. We apply the proposed voxel map to an iterated extended Kalman filter and construct a maximum a posteriori probability problem for pose estimation. Experiments on the open KITTI dataset show the high accuracy and efficiency of our method compared to other state-of-the-art methods. Outdoor experiments on unstructured environments with non-repetitive scanning LiDARs further verify the adaptability of our mapping method to different environments and LiDAR scanning patterns. Our codes and dataset are open-sourced on Github

I. INTRODUCTION

The paper addresses limitations of point-cloud maps by introducing adaptive probabilistic voxel mapping for efficient, uncertainty-aware LiDAR registration across varying environments and point densities.

  • Motivation: Point-cloud maps are simple but usually treat fitted planes as deterministic, ignoring uncertainty from LiDAR measurement noise.This makes the map representation less faithful to the noisy sensing process.
  • Motivation: Varying structures, LiDAR resolutions, scanning types, and sequential acquisition produce spatially and temporally changing point densities.These coarse-to-fine effects complicate feature tracking, uncertainty estimation, and map maintenance.
  • Contributions: The proposed adaptive voxel map uses different voxel sizes, tracks one plane feature per voxel, and repeatedly estimates feature parameters and uncertainties.This representation adapts to environment structure and point density while modeling uncertainty in both current points and the map.
  • Contributions: An octree-hash structure supports coarse-to-fine voxel construction, updates, and inquiries while targeting sparse and irregular point clouds.The contribution is positioned as an efficient alternative to more computationally demanding surfel association procedures.
  • Validation: The method is validated across structured and unstructured environments using multi-spinning and non-conventional solid-state LiDARs, including KITTI comparisons.The introduction claims superiority over other state-of-the-art methods on KITTI.
  • Relation to prior work: Unlike prior probabilistic surfel methods that omit pose-estimation uncertainty, this method models both LiDAR measurement noise and pose uncertainty.Compared with NDT, explicit planes support direct point-to-plane registration rather than penalizing all three spatial directions.

III. METHODOLOGY

The methodology builds probabilistic plane features from uncertain LiDAR points, accounting for both measurement and pose-estimation errors when representing points in the world frame.

  • Probabilistic plane representation: Each voxel contains one probabilistic plane feature, chosen because plane features are widely available in environments.The plane representation is used to model feature uncertainty within the voxel map.
  • Probabilistic plane representation: Plane uncertainty arises from noise in associated points, including raw LiDAR measurement noise and pose-estimation error during transformation to the world frame.Because the map is represented in the world frame, both local sensing and transformation uncertainty contribute.
  • Point uncertainty: A LiDAR point’s local-frame uncertainty combines ranging uncertainty with bearing-direction uncertainty.The bearing noise is modeled in the tangent plane of the measured bearing direction, while depth contributes ranging noise.
  • World-frame propagation: The point covariance is propagated into the world frame through the estimated rigid pose and its rotational and translational uncertainties.The resulting world-frame covariance is the basis for uncertainty-aware plane modeling.
  • Uncertainty behavior: Point uncertainty varies with range: closer points are dominated by ranging noise, whereas farther points are dominated by bearing noise.This range-dependent behavior motivates explicit uncertainty modeling for plane features.

2) Plane Uncertainty Modelling:

The paper models a plane feature from uncertain LiDAR points, representing it by a normal and a point while propagating their joint uncertainty. Because both parameters derive from the same noisy points, they are statistically dependent.

  • Plane feature construction: A plane feature is formed from LiDAR points with uncertainty caused by measurement noise and pose estimation error.The point covariance matrix is computed from the feature's point group.
  • Plane parameterization: The plane is represented by the covariance matrix's minimum-eigenvalue normal vector and the point q equal to the point centroid.The normal and centroid are functions of the constituent LiDAR points.
  • Uncertainty propagation: The ground-truth plane parameters are modeled as functions of noisy point realizations.The formulation explicitly perturbs each point before evaluating the plane parameters.
  • Uncertainty propagation: Derivatives of the plane parameters with respect to each point enable construction of the covariance matrix for the normal and point.The derivative uses the eigenstructure of the point covariance matrix.
  • Joint uncertainty: The normal and point are not independent because both are calculated from the same set of noisy points.Their shared point samples induce cross-dependence in the plane representation.

B. Coarse-to-fine and Efficient Voxel Map Construction

This section motivates a coarse-to-fine voxel-based map and presents the methodology for constructing and updating it efficiently.

  • Motivation: The section first explains why a coarse-to-fine voxel-based map is needed.The motivation precedes the construction methodology.
  • Construction: The section then describes how to build the voxel map efficiently.Construction is treated as a central part of the method.
  • Updates: The section also addresses efficient voxel-map updates.Updating the map is included alongside its initial construction.

1) Motivation:

LiDAR scans accumulate points from sparse to dense, making fine-to-coarse mapping inadequate when too few planes are available early. The paper motivates coarse-to-fine mapping and examines covariance convergence as more points arrive.

  • Motivation: LiDAR scans are sequentially accumulated from sparse to dense, especially in outdoor environments with widely distributed points.The sampling pattern creates strong variation in available point density during a scan.
  • Motivation: Fine-to-coarse surfel mapping may obtain very few planes from sparse point clouds, limiting use with lower-resolution or lower-rate LiDAR.The passage specifically links this limitation to insufficient point accumulation.
  • Motivation: The proposed coarse-to-fine strategy builds a rough voxel map when points are sparse and refines it as more points arrive.This directly addresses the changing density during scan accumulation.
  • Motivation: The convergence of the normal-vector covariance trace is evaluated as the number of points increases.Figure 4 uses point count as the varying quantity for this convergence analysis.

2) Voxel Map Construction :

The voxel map uses coarse root voxels with hashed indexing and finer sub-voxels containing plane features. Point matching propagates point and plane uncertainties, accepting only probabilistically consistent matches.

  • Voxel map construction: Populated coarse voxels are indexed by a Hash table, with an octree organizing their sub-voxels for coarse-to-fine construction.The map is initialized from the first scan in the global world frame.
  • Voxel map updates: During online odometry, new points construct unpopulated voxels or update plane parameters and uncertainty in existing voxels.Repeated updates can increase processing time as scans continue arriving.
  • Point matching: Point-to-plane matching accounts for uncertainty in the plane normal, plane center, and LiDAR point.The resulting probabilistic residual is used for scan matching and pose estimation.
  • Point matching: A predicted world-frame point is assigned to a root voxel by its Hash key, then its sub-voxels are searched for candidate planes.Each candidate plane is represented by a normal and center.
  • Point matching: A candidate match is accepted within 3σ, the highest-probability plane is chosen among multiple matches, and points failing the test are discarded.The rejection step removes possible false matches caused by voxel quantization.

D. State Estimation

The system fuses a state prior with valid point-to-plane measurements in an iterated extended Kalman filter to formulate a MAP pose estimate.

  • D. State Estimation: The LiDAR(-inertial) odometry system uses an iterated extended Kalman filter similar to FAST-LIO2.The prior comes from constant-velocity prediction for LiDAR-only odometry or IMU propagation for LiDAR-inertial odometry.
  • D. State Estimation: Each valid point-to-plane match is represented as an observation with a distance residual, observation function, and measurement noise.The observation noise is modeled as v_i ∼ (0, R_i).
  • D. State Estimation: The observation model is linearized around the current state update after substituting the sensor pose into the point-to-plane measurement.The state x_k is identified with the sensor pose T_k.
  • D. State Estimation: The MAP estimate combines the state prior with all effective measurement observations.The objective contains a prior term and a measurement-observation term.

IV. EXPERIMENTS

The experiments evaluate accuracy, efficiency, and adaptability across three LiDAR types and three environments, with comparisons against state-of-the-art methods.

  • IV. EXPERIMENTS: Experiments cover urban, indoor, and unstructured outdoor environments using three typical LiDARs.The LiDAR details are reported in Table I.
  • IV. EXPERIMENTS: Each experiment compares the proposed method with state-of-the-art counterparts.

A. Urban Environment Test

On KITTI urban sequences, the full method achieved the strongest reported trajectory accuracy while also providing efficient scan processing. Ablations indicate that adaptive voxelization and especially probabilistic plane representation drive the accuracy gains.

  • Accuracy: On KITTI training sequences, Ours (full) achieved the best overall ATE among the compared methods, particularly on long-term sequences 00, 02, and 08.The comparison included FAST-LIO2, MULLS, LiTAMIN2, Suma, and Lego-Loam.
  • Accuracy: The estimated trajectories for sequences 00 to 10 were reported as close to ground truth across the evaluated sequences.The figure overlays method trajectories with ground-truth trajectories and marks the starting point.
  • Ablation: Removing adaptive voxelization degraded performance because the full method captures plane features across large and fine spatial scales.The fixed-size ablation used 2m voxels, while the full method used adaptive voxel sizes.
  • Ablation: Removing probabilistic plane representation caused a larger accuracy degradation than removing adaptive voxelization.Without probabilistic plane representation, performance was reported at the same level as FAST-LIO2, which also uses deterministic planes and the same IKFoM framework.
  • Map analysis: Plane uncertainty was higher in distant, cluttered, and moving-object regions than near roads, structured areas, and static objects.The paper attributes these patterns to uneven point measurements and the differing structure of observed regions.
  • Runtime: The method had the lowest processing time per LiDAR scan among the compared systems.The reported efficiency advantages include avoiding feature extraction and real-time or periodic kd-tree construction.

B. Indoor Environment Test

In indoor laboratory and warehouse tests with a solid-state LiDAR, the method was evaluated against SSL SLAM using shared settings. It achieved better reported accuracy and efficiency, while the warehouse map appeared more precise.

  • Experimental setup: The indoor experiment used an Intel L515 solid-state LiDAR in laboratory and warehouse environments, with routes starting and ending at the same place.The method was compared with SSL SLAM using the same maximum voxel size and octree-layer settings across datasets.
  • Comparison: Compared with SSL SLAM, the proposed method achieved better end-to-end accuracy and efficiency on the indoor datasets.The paper attributes this comparison to avoiding feature extraction and modeling uncertainty in both LiDAR points and plane maps.
  • Comparison: On the SSL SLAM paper’s dataset, the method reported 0.12m / 5.1ms versus 0.15m / 25.1ms for SSL SLAM.The paper describes this as a small improvement because that dataset involved slower, less challenging motion.
  • Map quality: The warehouse colored point-cloud map from the proposed method was more precise and less blurred than SSL SLAM’s, especially near the return route.This comparison is reported from a supplementary-material figure.
  • Additional evaluation: The paper further tested adaptability in park and mountain environments using a non-repetitive scanning Livox Avia LiDAR with built-in IMU.The sensor provided 10 Hz LiDAR and 200 Hz IMU data, with the IMU used for motion-distortion compensation and motion prior.

1) Park Environment:

The experiments evaluate the mapping method in park and mountain environments, including long handheld trajectories and UAV-based ground scans. The method achieves higher accuracy than Faster-LIO and FAST-LIO2 while maintaining comparable computation time, and its probabilistic map represents uncertainty differences across environmental structures.

  • Park Environment: 485 m and 815 m trajectories were collected with a handheld LiDAR–IMU device in a park, with routes starting and ending at the same place to expose drift.Two trajectories followed the park periphery, while the longest also covered the interior.
  • Park Environment: The UAV experiment targeted a mountain environment that was less structured and more challenging than the park.
  • Park Environment: Large highway planes had low uncertainty, whereas small vegetation planes had high uncertainty in the mountain map.
  • Park Environment: Our method achieved much higher accuracy than Faster-LIO and FAST-LIO2 across all three Livox Avia sequences.The comparison used end-to-end error and average computation time per scan.
  • Park Environment: Our method and Faster-LIO had more comparable computation times than FAST-LIO2 because both organized their maps with a Hash table.The probabilistic method required additional point–plane distribution calculations but could reject further outliers.
  • Park Environment: The method also performed well with non-repetitive small-FoV LiDAR in unstructured parks and mountains.
Loading 2109.07082v5…