PaperlayerView exact revision on arXiv

Source-linked AI summary

FAST-LIO2: Fast Direct LiDAR-inertial Odometry

Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, Fu Zhang

arXiv:2107.06829v1cs.RO

TL;DR

FAST-LIO2 addresses real-time dense 3D LiDAR mapping and localization by directly registering raw points and using the incremental ikd-Tree data structure. It achieves faster computation with competitive or better accuracy, while supporting robust operation in sparse scenes and fast motion.

  • Problem

    Real-time dense 3D mapping and localization remain difficult under onboard computation constraints, while feature-based vision methods provide sparse maps and suffer from illumination variation and motion blur.

  • Method

    FAST-LIO2 directly registers raw LiDAR points to the map and uses ikd-Tree for incremental insertion, deletion, downsampling, and dynamic rebuilding.

  • Results

    Across open datasets, FAST-LIO2 is significantly faster than state-of-the-art LiDAR-inertial odometry while achieving highly competitive or better accuracy; ikd-Tree achieves the best overall data-structure performance.

  • Takeaways & Limitations

    The framework's efficient mapping supports increased accuracy and robustness in fast motion and sparse scenes, including cases with small fields of view or few prominent features.

Abstract

from arXiv · show

This paper presents FAST-LIO2: a fast, robust, and versatile LiDAR-inertial odometry framework. Building on a highly efficient tightly-coupled iterated Kalman filter, FAST-LIO2 has two key novelties that allow fast, robust, and accurate LiDAR navigation (and mapping). The first one is directly registering raw points to the map (and subsequently update the map, i.e., mapping) without extracting features. This enables the exploitation of subtle features in the environment and hence increases the accuracy. The elimination of a hand-engineered feature extraction module also makes it naturally adaptable to emerging LiDARs of different scanning patterns; The second main novelty is maintaining a map by an incremental k-d tree data structure, ikd-Tree, that enables incremental updates (i.e., point insertion, delete) and dynamic re-balancing. Compared with existing dynamic data structures (octree, R*-tree, nanoflann k-d tree), ikd-Tree achieves superior overall performance while naturally supports downsampling on the tree. We conduct an exhaustive benchmark comparison in 19 sequences from a variety of open LiDAR datasets. FAST-LIO2 achieves consistently higher accuracy at a much lower computation load than other state-of-the-art LiDAR-inertial navigation systems. Various real-world experiments on solid-state LiDARs with small FoV are also conducted. Overall, FAST-LIO2 is computationally-efficient (e.g., up to 100 Hz odometry and mapping in large outdoor environments), robust (e.g., reliable pose estimation in cluttered indoor environments with rotation up to 1000 deg/s), versatile (i.e., applicable to both multi-line spinning and solid-state LiDARs, UAV and handheld platforms, and Intel and ARM-based processors), while still achieving higher accuracy than existing methods. Our implementation of the system FAST-LIO2, and the data structure ikd-Tree are both open-sourced on Github.

I. INTRODUCTION

LiDAR-based SLAM must provide accurate, low-latency state estimation and dense maps despite massive point streams and limited onboard computation. FAST-LIO2 addresses these challenges with direct raw-point registration and the incremental ikd-Tree map structure.

  • Motivation: LiDAR-based SLAM must process hundreds of thousands to millions of points per second while producing accurate, low-latency state estimates and dense maps.These demands are especially challenging on computationally constrained robotic platforms.
  • Motivation: Feature extraction reduces computation but can produce few points in structure-less environments and small-FoV solid-state LiDAR scans.Feature extraction also varies with the LiDAR scanning pattern.
  • Contributions: FAST-LIO2 directly registers raw LiDAR points to the map, eliminating hand-engineered feature extraction and improving registration in aggressive motion and cluttered environments.The approach also exploits subtle environmental structure and adapts to different scanning patterns.
  • Related Work: FAST-LIO2 inherits tightly coupled LiDAR-inertial fusion and addresses limitations of feature-based scan-to-scan or small-local-map odometry.Earlier systems use feature points and may require additional mapping or smoothing processes to control drift.
  • Contributions: The ikd-Tree supports efficient nearest-neighbor search, point insertion, deletion, on-tree downsampling, and dynamic re-balancing for real-time mapping.Parallel rebuilding avoids significant delays while preserving real-time operation and accuracy.

A. Kinematic Model

The kinematic model defines the LiDAR–inertial state on a manifold, using IMU measurements and noise to propagate motion. The measurement model projects LiDAR points into the global frame and constrains them to local map planes.

  • The kinematic model uses IMU acceleration and angular-velocity measurements, measurement noise, and random-walk IMU biases.
  • The continuous kinematic model is discretized at each IMU sampling period ∆t using the manifold ⊞ operation.
  • The system state lies on M ≜ SO(3) × R15 × SO(3) × R3 with dimension 24.
  • Back-propagation estimates each LiDAR point’s pose relative to scan-end time, correcting motion distortion caused by sequential sampling during motion.
  • Each transformed LiDAR point is modeled as lying on a local map plane defined by normal Guj and point Gqj, yielding an implicit measurement model.

B. Iterated Kalman Filter

FAST-LIO2 propagates the state with IMU measurements and performs iterated LiDAR updates on the same manifold. Raw LiDAR points are associated with local planes in an ikd-Tree map to construct measurement residuals.

  • The iterated Kalman filter performs IMU-based propagation and LiDAR-scan updates directly on the manifold M, avoiding state renormalization.
  • Multiple IMU propagation steps usually precede each LiDAR update because IMU measurements arrive at a higher frequency.The passage gives example rates of 200Hz for IMU and 10Hz ∼100Hz for LiDAR.
  • During propagation, the filter advances the state and covariance from the previous LiDAR estimate using the IMU input with process noise set to zero.
  • For each LiDAR point, the filter searches the ikd-Tree map for five nearest neighbors and fits a local plane patch.
  • The plane normal, centroid, Jacobian, and raw measurement noise define the linearized LiDAR residual used in the update.

3) Iterated Update:

The iterated update combines a propagated Gaussian prior with linearized LiDAR measurements to obtain a posterior state estimate. After convergence, the updated state transforms the scan points and inserts them into the map.

  • The measurement distribution is combined with the prior to form the posterior state distribution and its maximum a posteriori estimate.
  • The MAP problem is solved with an iterated Kalman filter using stacked measurement Jacobians and block-diagonal measurement covariance.
  • The Kalman gain computation inverts a matrix of state dimension rather than measurement dimension.
  • After convergence, the optimal state transforms the current LiDAR points to the global frame, where the transformed points are inserted into the ikd-Tree map.

V. MAPPING

FAST-LIO2 maintains a bounded local map in an incremental k-d tree while supporting state-estimation inputs and dynamic region management. The ikd-Tree stores points throughout the tree to support insertion, deletion, and re-balancing.

  • The mapping section covers incremental insertion, deletion, k-nearest-neighbor search, and time-complexity analysis for ikd-Tree.
  • The map grows by merging each new scan at odometry rate, while only points within a local region of length L around the LiDAR are retained.
  • The mapping pipeline receives current LiDAR points and IMU inputs, performs propagation and motion compensation, and outputs the optimal state and covariance.
  • When the detection area reaches the map boundary, the local map region moves and points in the subtraction area are removed from ikd-Tree.
  • Unlike many k-d trees that store points only at leaves, ikd-Tree stores points at both leaves and internal nodes to support dynamic insertion and re-balancing.

8 end

The ikd-Tree uses median-based recursive partitioning and supports incremental point and cuboid operations, with insertion integrated with resolution-controlled downsampling.

  • Construction: The ikd-Tree recursively splits space at the median point along the longest dimension until each subspace contains one point.Tree size and range attributes are initialized during construction.
  • Incremental operations: Incremental operations include point-wise insertion, deletion, and reinsertion, plus equivalent operations on all points within an axis-aligned cuboid.The cited passage distinguishes point-wise and box-wise operations.
  • Downsampling: Point insertion is integrated with on-tree downsampling to maintain the map at a predetermined resolution.The method keeps the point nearest the downsampling cube’s center.
  • Insertion procedure: The insertion procedure recursively descends from the root, compares the new point along division axes, and updates visited-node attributes.The supplied algorithm fragments show initialization and left- or right-child recursion.

9 Algorithm End

The update algorithms handle insertion and deletion through recursive tree traversal, lazy deletion labels, cuboid-range pruning, and attribute maintenance.

  • Point insertion: An empty node is initialized for insertion, while nonempty nodes route the point left or right according to its coordinate on the division axis.The algorithm fragments specify node initialization and both recursive branches.
  • Deletion: Deleted points remain in the tree temporarily under lazy labels and are removed during a later rebuilding process.The labels deleted and treedeleted track point- and subtree-level deletion status.
  • Box-wise deletion: Box-wise deletion recursively compares subtree and target cuboids, allowing nonintersecting subtrees to be pruned.The operation is represented by recursive calls on the left and right children.
  • Attribute maintenance: After each incremental operation, visited-node attributes are recomputed from child and node information, including size, invalid count, range, and deletion status.The range is merged from child ranges and the node’s stored point information.

D. Re-balancing

The ikd-Tree monitors balance and deletion criteria, rebuilding affected subtrees while using parallel rebuilding to preserve queries and mapping throughput.

  • Balancing criterion: The tree uses α-balanced and α-deleted criteria to monitor height and the number of invalid nodes in each subtree.The criteria use α_bal, α_del, subtree size S(T), and invalid-node count I(T).
  • Balancing criterion: Violating either criterion triggers subtree rebuilding, which limits height and removes deleted nodes to support efficient future operations and queries.The maximum height of an α-balanced tree is log1/α_bal(n).
  • Rebuild: Rebuilding flattens valid points, discards deleted nodes, and constructs a new perfectly balanced k-d tree.Large-subtree rebuilding can run in a second thread.
  • Parallel rebuild: During parallel rebuilding, updates are logged and replayed on the new tree, while queries continue using the original subtree.The second thread locks updates but not queries during flattening, then applies logged operations after reconstruction.

24 End Function

The parallel rebuild design preserves uninterrupted mapping, while optimized nearest-neighbor search and low-dimensional complexity analyses characterize ikd-Tree efficiency.

  • Parallel rebuild: During second-thread rebuilding, mapping continues at the odometry rate without interruption, although efficiency temporarily decreases because the tree is unbalanced.Queries remain available while updates are locked.
  • Nearest-neighbor search: Nearest-neighbor search uses subtree range bounds and a priority queue to prune backtracking, and supports multithreaded execution.The bounds-overlap-ball test compares lower bounds with the current maximal neighbor distance.
  • Complexity analysis: The complexity analysis assumes low-dimensional data, such as the three dimensions used in FAST-LIO2.The analysis separates incremental operations, rebuilding, and k-nearest-neighbor search.
  • Complexity analysis: Box-wise delete and search are analyzed through a function H(n), with lazy deletion adding O(1) overhead to range-search complexity.The proof treats box-wise deletion as range search with lazy labels.
  • Complexity analysis: For a tree of size n, point insertion with on-tree downsampling has time complexity O(log n).The result combines downsampling and insertion costs under the stated assumptions.

2) Re-build:

The benchmark evaluates ikd-Tree and FAST-LIO2 across diverse LiDAR datasets, emphasizing computational efficiency for incremental mapping and nearest-neighbor search. ikd-Tree achieves the best overall data-structure performance while FAST-LIO2 substantially reduces processing time against other LIO methods.

  • 2) Re-build:: O(n log n) single-thread rebuilding and O(log n) expected kNN search characterize the ikd-Tree’s computational design.The tree height is bounded logarithmically, and kNN backtracking is proportional to a constant independent of tree size.
  • 2) Re-build:: 19 sequences from five datasets evaluate FAST-LIO2 using both solid-state and spinning LiDARs.The benchmarks include varied platforms, sensors, urban settings, and sequences with moving vehicles.
  • 2) Re-build:: Across 18 sequences, ikd-Tree maintains efficient insertion with on-tree downsampling and kNN search as tree size increases.Figure 5 measures average per-target-point insertion and kNN-search time across different tree sizes.
  • 2) Re-build:: ikd-Tree achieves the best overall performance among the compared dynamic data structures when incremental updates and kNN search are considered together.Octree is often fastest for incremental updates, but ikd-Tree performs best for kNN search and overall performance.
  • 2) Re-build:: Nanoflann’s masked deletions cause tree growth and severe insertion-time peaks on long sequences.Its tree exceeds 6 × 10^6 points in utbm and 10^7 in nclt, while ikd-Tree’s maximal incremental-update time remains 214.4 ms in nclt 2 and below 150 ms elsewhere.

C. Accuracy Evaluation

FAST-LIO2 delivers higher accuracy than competing LiDAR-inertial systems across benchmark sequences while using substantially less computation. Its direct registration and incremental mapping remain effective across varied datasets and platforms.

  • RMSE Benchmark: The direct method outperforms the feature-based FAST-LIO2 variant in most sequences, with only negligible differences on nclt 4 and nclt 6.The comparison supports using raw-point registration rather than feature extraction for these benchmarks.
  • RMSE Benchmark: FAST-LIO2 or its variant achieves the best performance in 18 of 19 sequences and is the most robust method overall.The only exception is ulhk 4, where LILI-OM is slightly more accurate.
  • Drift Benchmark: FAST-LIO2 or its variants achieve the lowest drift in 5 of 7 sequences under the end-to-end error benchmark.FAST-LIO2 keeps the same parameters across sequences, whereas LILI-OM tunes parameters for its own lili sequences.
  • Processing-Time Evaluation: FAST-LIO2 consumes x8 less time than LILI-OM, x10 less than LIO-SAM, and x6 less than LINS.Its integrated odometry-and-mapping total includes motion compensation, nearest-neighbor search, state estimation, and mapping.
  • Processing-Time Evaluation: FAST-LIO2 processes 756 points per scan in 1.82 ms on Intel and 5.23 ms on ARM, while FAST-LIO mapping exceeds 10 ms per scan in a large scene.The incremental map updates avoid repeatedly rebuilding a k-d tree as the map grows.

2) Aggressive UAV Flight Experiment:

Real-world tests evaluate FAST-LIO2 during aggressive UAV flight, fast handheld motion, and aerial mapping. The system maintains accurate, real-time mapping and state estimation across these demanding platforms and environments.

  • Aggressive UAV Flight Experiment: During the UAV flip, FAST-LIO2 agrees well with the actual pose while maintaining real-time environmental mapping.The experiment uses a small quadrotor and an onboard controller receiving state feedback from FAST-LIO2.
  • Aggressive UAV Flight Experiment: The flip reaches an average angular velocity of 912 deg/s and a maximum of 1198 deg/s.These values occur from 50.8 s to 51.2 s in the experiment.
  • Fast-Motion Handheld Experiment: The fast-motion handheld experiment reaches 7 m/s and approximately ±100 deg/s, yet its 81 m trajectory has less than 0.06 m end-to-end error.The experiment starts and ends at the same point.
  • Airborne Mapping: In three aerial-mapping environments, FAST-LIO2 produces maps containing fine vegetation and road structures, with average scan times of 19.6 ms, 23.9 ms, and 23.7 ms.The UAV uses GPS/IMU for its own navigation, not as input to FAST-LIO2; GPS trajectories were unavailable for quantitative evaluation.

APPENDIX

The appendix provides detailed information about all 37 sequences used in the paper’s Section VI evaluation.

  • APPENDIX: Table VIII lists detailed information for all 37 sequences used in Section VI.
Loading 2107.06829v1…