PaperlayerView exact revision on arXiv

Source-linked AI summary

Direct LiDAR Odometry: Fast Localization with Dense Point Clouds

Kenny Chen, Brett T. Lopez, Ali-akbar Agha-mohammadi, Ankur Mehta

arXiv:2110.00605v3cs.RO

TL;DR

Modern LiDAR scans can overwhelm computationally limited processors, challenging real-time localization in perceptually difficult environments. DLO addresses this with efficient keyframe-based submapping and a recycled-data-structure registration solver, achieving accurate localization with low computational overhead. The method was evaluated on aerial and legged robots in challenging field environments.

  • Problem

    Modern LiDAR sensors generate many depth returns, making real-time odometry difficult for computationally limited robotic platforms.

  • Method

    DLO uses adaptive keyframing, keyframe-based submapping, and NanoGICP with data structure recycling to register dense, minimally preprocessed point clouds.

  • Results

    21.9ms average processing time and 9.5% CPU load prevented LiDAR frame drops under the full data-structure recycling scheme.

  • Takeaways & Limitations

    DLO provides a lightweight frontend localization solution evaluated through benchmarks and field experiments on computationally limited robotic platforms in challenging environments.

Abstract

from arXiv · show

Field robotics in perceptually-challenging environments require fast and accurate state estimation, but modern LiDAR sensors quickly overwhelm current odometry algorithms. To this end, this paper presents a lightweight frontend LiDAR odometry solution with consistent and accurate localization for computationally-limited robotic platforms. Our Direct LiDAR Odometry (DLO) method includes several key algorithmic innovations which prioritize computational efficiency and enables the use of dense, minimally-preprocessed point clouds to provide accurate pose estimates in real-time. This is achieved through a novel keyframing system which efficiently manages historical map information, in addition to a custom iterative closest point solver for fast point cloud registration with data structure recycling. Our method is more accurate with lower computational overhead than the current state-of-the-art and has been extensively evaluated in multiple perceptually-challenging environments on aerial and legged robots as part of NASA JPL Team CoSTAR's research and development efforts for the DARPA Subterranean Challenge.

I. INTRODUCTION

DLO targets real-time LiDAR localization when dense sensor scans overwhelm computationally limited processors. It combines speed-first keyframing, submapping, and registration innovations to use minimally preprocessed clouds while maintaining accurate pose estimation.

  • Motivation: Modern LiDAR odometry must process many depth returns in real time, creating computational bottlenecks for high-rate state estimation.Frame drops during alignment can degrade pose estimation.
  • Contribution: DLO directly uses dense, minimally preprocessed point clouds for real-time frontend localization on consumer-grade processors.The system optionally incorporates an IMU prior.
  • Contribution: An adaptive keyframing system captures significant environmental information using a novel spaciousness metric.
  • Contribution: A keyframe-based submapping method uses convex optimization to generate permissive local submaps for global pose refinement.
  • Contribution: NanoGICP is a custom iterative closest point solver that recycles data structures to eliminate redundant calculations during lightweight scan matching.

II. METHODS

The method estimates robot pose from successive 3D LiDAR scans and maintains a world-frame map. Its processing pipeline lightly filters input clouds, performs staged alignment, and updates keyframes and map structures.

  • Notation and pipeline: DLO estimates the robot’s current pose and map from adjacent point clouds using an SE(3) transformation in world coordinates.
  • Preprocessing: Input preprocessing removes self-points with a 1m^3 box filter and applies a 0.25m 3D voxel grid filter.The filters are intended to minimize information loss from raw sensor data.
  • Pipeline: The algorithm processes each scan through preprocessing, adaptive-parameter computation, initialization, optional IMU use, scan-to-map alignment, and map or keyframe updates.
  • Map management: The map is updated by adding the newly selected keyframes to the previous map and propagating data structures.

1) Scan-to-Scan:

The scan-to-scan stage estimates relative motion by aligning adjacent source and target clouds. GICP minimizes correspondence residuals, optionally initialized with an IMU-derived prior.

  • Relative alignment: Scan-to-scan matching computes a relative transform between adjacent source and target point clouds.
  • GICP objective: The GICP residual error uses corresponding points and their estimated source and target covariance matrices.
  • Initialization: An external IMU prior can initialize the objective to push convergence toward a global minimum.
  • Initialization: Without an available prior, the system reverts to pure LiDAR odometry and relies solely on point-cloud correspondence matching.

2) Scan-to-Map:

Scan-to-map matching refines the scan-to-scan motion estimate by registering the current cloud against a derived historical submap. DLO derives this submap in keyframe-space rather than querying nearby map points directly.

  • Scan-to-Map: Scan-to-map matching refines the previous motion estimate by aligning the current source cloud P^s_k with a local submap S_k.The resulting pose is the final estimated robot pose used by downstream modules.
  • Scan-to-Map: The scan-to-map objective minimizes GICP residual error across corresponding points between P^s_k and S_k.The optimization is initialized with the propagated scan-to-scan result from L to W.
  • Scan-to-Map: The scan-to-map stage compares the propagated motion prior against historical map data to improve global consistency.Its output is the final world-frame pose estimate.
  • Scan-to-Map: DLO generates S_k by associating scans with keyframes and stitching selected point clouds instead of retrieving points within a radius around the robot.This keyframe-space search is described as faster, more consistent, and more permissive than radius-based submap generation.

D. Optimization Prior

DLO can initialize scan alignment with an external motion prior, particularly from IMU gyroscope measurements. The prior supplies rotational information while leaving translational acceleration integration for future work.

  • Optimization Prior: The scan-to-scan nonlinear optimization can use a prior representing the estimated relative motion between adjacent LiDAR frames.The prior is intended to reduce convergence to a sub-optimal local minimum.
  • Optimization Prior: IMU angular velocity measurements provide a rotational prior through calibrated gyroscopic propagation of quaternion kinematics.Only gyroscope measurements between the current and previous LiDAR scans are integrated.
  • Optimization Prior: The keyframe-based submapping approach supports scan-to-map alignment by concatenating keyed scans and anchoring distant current-scan points.This provides the historical map information used after scan-to-scan alignment.
  • Optimization Prior: The propagated quaternion is converted into an SE(3) matrix with zero translational component for scan-to-scan initialization.The translational prior from accelerometer measurements is left for future work.

E. Fast Keyframe-Based Submapping

DLO accelerates local submap construction by searching over a history of keyframes and concatenating selected scans, rather than querying dense point-cloud space. The resulting submaps improve overlap with both nearby and distant current-scan points.

  • Fast Keyframe-Based Submapping: DLO stores historical keyframes linked to their point-cloud scans and constructs local submaps by concatenating scans from selected keyframes.This replaces direct retrieval of local points from a cumulative map.
  • Fast Keyframe-Based Submapping: Keyframe-space searches typically involve only a few hundred points, providing more consistent computational performance than radius searches over hundreds of thousands of map points.The stated benefit is reduced likelihood of dropping frames.
  • Fast Keyframe-Based Submapping: Adaptive keyframing decreases the threshold while traversing a narrow ramp to capture small-scale environmental details.The adaptive threshold is described as improving robustness to changing environmental dimension.
  • Fast Keyframe-Based Submapping: A keyframe-derived submap can overlap more with the current scan because its size depends on LiDAR range rather than a predetermined radius.The figure contrasts this with a fixed r = 20m radius submap that may insufficiently overlap the current scan.

1) Keyframe Selection via kNN and Convex Hull:

DLO forms each submap from nearby keyframes and keyframes on the set’s convex hull. This combines local overlap with boundary points that anchor more distant scan regions.

  • Keyframe Selection via kNN and Convex Hull: S_k is defined as the concatenation of K nearest-neighbor keyframe scans Q_k and L nearest-neighbor convex-hull scans H_k: S_k = Q_k ⊕ H_k.The convex-hull indices are keyframes forming the intersection of all convex sets containing the relevant keyframes.
  • Keyframe Selection via kNN and Convex Hull: The selected keyframes are visualized as green spheres, and their concatenated submap is shown in red.The figure depicts the keyframes composing the extracted submap.
  • Keyframe Selection via kNN and Convex Hull: Nearest-neighbor keyframes target overlap with nearby current-scan points, while convex-hull keyframes add boundary map points for more distant points.Together they provide multiple scales of environmental features for alignment.
  • Keyframe Selection via kNN and Convex Hull: Keyframes classified as both nearest neighbors and convex-hull indices are used only once in the submap.This avoids duplicating the same scan during concatenation.

2) Adaptive Keyframing:

DLO adapts keyframe spacing to the surrounding point-cloud scale, while reusing previously computed submap normals to reduce repeated computation.

  • Adaptive keyframe placement: Keyframe placement is adapted to environmental structure because large spaces preserve prominent features longer, whereas narrow spaces require denser keyframes.The translational threshold is scaled using a smoothed median point distance.
  • Adaptive keyframe placement: mk = αm_k−1 + βM_k smooths the median Euclidean point distance used to scale the translational keyframe threshold.The paper sets α = 0.95 and β = 0.05.
  • Adaptive keyframe placement: The translational threshold ranges from 0.5m to 10m according to the smoothed environmental distance signal.The rotational threshold remains fixed at 30°.
  • Submap normal approximation: Submap normals are reconstructed by concatenating normals from constituent keyframes instead of recomputing them for every iteration.This approximates the submap covariance information while avoiding explicit normal computation for each submap.

2) Data Structure Recycling:

DLO recycles kdtrees, covariances, and keyframe-derived structures across scan-matching modules to remove redundant operations and reduce system overhead.

  • Map construction: DLO constructs dense point-cloud maps by transforming scans into a world frame and concatenating them with voxel filtering.The Alpha-course map is generated from estimated positions at each timestamp.
  • Shared data structures: The pipeline requires eight data elements for scan-to-scan and scan-to-map matching, including kdtrees and covariance matrices for source and target clouds.These structures support correspondence search and GICP alignment.
  • Kdtree recycling: The source kdtree is built once per scan and shared across both modules, while the previous scan’s source tree becomes the next target tree.This avoids rebuilding structures for the same scan data.
  • Kdtree recycling: The scan-to-map target tree is rebuilt only when the selected keyframe set changes; otherwise, it is reused.The selection changes when the kNN and convex-hull keyframe set differs between iterations.
  • NanoGICP integration: NanoGICP combines FastGICP and NanoFLANN with modifications that enable data-structure sharing between separate scan-matching solver instances.The two instances use different hyperparameters for scan-to-scan and scan-to-map matching.

III. RESULTS

Experiments on challenging subterranean data evaluate submapping and recycling choices using accuracy, processing time, CPU load, and dropped scans.

  • Experimental setup: The Alpha Course evaluation uses 60 minutes of Velodyne VLP-16 and VectorNav VN-100 data from an abandoned powerplant with large or self-similar scenes.The study examines keyframe-based submapping, normal approximation, and data-structure reuse.
  • Field evaluation: The field evaluation includes autonomous drone mapping in a low-light, obstructed, wet mine and quadruped mapping across three levels of an abandoned subway.The subway traversal covered 856m.
  • Submapping results: 74.2ms per scan and 37.5% CPU load for radius-based submapping decreased to 21.6ms / 10.2% and 19.1ms / 9.1% for static and adaptive keyframing.The three schemes were compared using absolute pose error, processing time, and CPU load.
  • Recycling results: 69.8ms per scan with nearly 10% of scans dropped in the naive recycling scheme decreased to 21.9ms and 9.5% CPU load with full recycling, preventing dropped frames.Partial recycling of kdtrees or covariances provided smaller improvements than the full scheme.

3) NanoGICP:

NanoGICP is benchmarked against open-source GICP implementations, while DLO is evaluated against multiple odometry systems and deployed on aerial and legged robots.

  • NanoGICP benchmark: NanoGICP convergence time is compared across 100 benchmark alignments against PCL GICP and FastGICP’s multithreaded implementation.All algorithms use an identity initialization prior.
  • Benchmark results: DLO is compared with BLAM, Cartographer, LIO-Mapping, LOAM, and LOCUS on the Alpha and Beta Subterranean Challenge courses.The comparison reports odometry accuracy and CPU load.
  • Benchmark results: DLO uses less than one CPU core on average and at peak, while reporting lower APE and ME than the compared methods.The paper attributes these observations to submap derivation and extensive internal data-structure reuse.
  • Benchmark results: More than twice the accuracy was achieved in the Beta course for maximum, mean, and standard-deviation error, even without motion-distortion correction.The paper also reports fewer dropped frames and higher-resolution matching of dense point clouds.
  • Field deployment: DLO was integrated on an Ouster-equipped aerial vehicle and a Velodyne-equipped Spot using a VectorNav IMU and an Intel NUC processor.The systems were tested in manual and autonomous traversals.
  • Conclusion: The paper presents DLO as lightweight frontend localization for long-term traversals, combining keyframe-point-cloud submaps with shared solver structures through NanoGICP.Benchmarks and field experiments cover multiple platforms and challenging environments.
Loading 2110.00605v3…