FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

Wei XuYixi CaiDongjiao HeJiarong LinFu Zhang

article2021IEEE Transactions on robotics1,772 citations

Introduces a direct LiDAR-inertial odometry framework that matches raw point clouds using a dynamic incremental k-d tree, enabling highly accurate mapping at up to 100 Hz without hand-engineered feature extraction across diverse LiDAR sensors and embedded processors.

Listen

Autonomous robots, drones, and self-driving vehicles require rapid, accurate 3D mapping and self-localization to navigate safely in unknown environments. While laser-based distance sensors, known as LiDAR, provide direct and accurate spatial data, processing millions of points per second on resource-constrained onboard computers creates severe computational bottlenecks. Existing solutions typically extract handcrafted geometric features (such as edges and planes) to reduce data volume and limit map updates, but this approach struggles in unstructured environments, degrades with newer solid-state sensors that have narrow fields of view, and introduces significant software engineering overhead.

The article evaluates FAST-LIO2, an advanced LiDAR-inertial navigation framework designed to deliver high-speed, highly accurate, and robust mapping and state estimation without handcrafted feature extraction. The article demonstrates how combining direct raw point cloud registration with an incremental map data structure, named ikd-Tree, overcomes the computational limitations of conventional systems across various hardware platforms and operational environments.

To evaluate this approach, the authors integrated an iterated Kalman filter that mathematically optimizes sensor fusion and corrects motion distortion using an inertial measurement unit. Global map points are dynamically maintained within a local bounding region via the ikd-Tree structure, which supports concurrent rebalancing and on-tree downsampling. The authors conducted extensive benchmark comparisons across 19 public dataset sequences from five distinct datasets, comparing FAST-LIO2 against leading LiDAR-inertial frameworks (LILI-OM, LIO-SAM, and LINS). They also evaluated the ikd-Tree against standard spatial data structures across 18 sequences and validated the full pipeline in real-world aerial, handheld, and aggressive drone flight experiments running on Intel and ARM processors.

The experimental results highlight significant performance and efficiency advantages. FAST-LIO2 consistently outperformed competing systems, achieving the highest localization accuracy in 18 out of 19 benchmark sequences and reducing drift to less than 0.1 meters in several closed-loop trials. Computationally, FAST-LIO2 ran roughly 6 to 10 times faster than competing algorithms, requiring only 1.82 milliseconds per scan on standard onboard processors and achieving up to 100 Hz real-time update rates. Furthermore, the ikd-Tree prevented severe processing latency spikes, maintaining update times below 215 milliseconds on massive point clouds where alternative methods caused multi-second delays. The system remained robust under extreme operational conditions, including aggressive drone maneuvers reaching angular velocities near 1,200 degrees per second.

These findings demonstrate that robotic systems can eliminate manual feature tuning and separate low-rate mapping modules, significantly reducing development complexity and operational latency. Achieving real-time 10 Hz performance on low-power ARM architectures lowers hardware costs, payload weight, and power consumption for autonomous platforms. Because the direct method bypasses sensor-specific scanning geometry, engineering teams can deploy different LiDAR sensor types interchangeably without redesigning feature extraction algorithms.

Engineering teams should consider adopting direct LiDAR-inertial frameworks and incremental spatial trees for autonomous navigation and mapping deployments, particularly on power- and weight-sensitive platforms such as micro-drones. The authors have open-sourced both FAST-LIO2 and the ikd-Tree structure to facilitate adoption. Stakeholders should note that FAST-LIO2 operates purely as an odometry framework without global loop closure or graph optimization, meaning systematic drift may still accumulate over very large spatial trajectories. While confidence in the benchmarked accuracy and computational efficiency is high across tested environments, applications requiring long-duration mission autonomy should pilot FAST-LIO2 alongside complementary global loop-closing modules to maintain drift-free maps.

University of Hong Kong feature extraction handheld inertial navigations fast direct LiDAR-inertial odometry FAST-LIO2 LiDAR-inertial navigation systems

arXiv: 2107.06829 hku-mars/FAST_LIO

Paper: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping, Tixiao Shan et al. (2020). Presents tightly-coupled LiDAR-inertial odometry via factor graph optimization and IMU pre-integration, establishing the state-of-the-art baseline and motivation that FAST-LIO2 improves upon.
Paper: Iterative point matching for registration of free-form curves and surfaces, Zhengyou Zhang (1994). Introduces fundamental point-to-point iterative nearest-neighbor registration principles that form the mathematical basis for direct point registration in LiDAR mapping.
Paper: Estimating uncertain spatial relationships in robotics, Randall Smith et al. (1986). Formulates the foundational stochastic mapping and spatial uncertainty state-estimation framework underpinning modern Kalman filter-based SLAM architectures.
Paper: Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age, Cesar Cadena et al. (2016). Provides a comprehensive survey of SLAM representations, filtering versus optimization paradigms, and metric map tracking requirements essential for contextualizing direct LiDAR-inertial odometry.

No sufficiently relevant recommendations were found.

Cover for FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

Abstract

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.

Topics

feature extraction handheld inertial navigations kalman filters lidar systems odometry pose estimation tightly-coupled imu inertial measurements

Keywords

fast direct LiDAR-inertial odometry FAST-LIO2 LiDAR-inertial navigation systems solid-state LiDAR point clouds airborne mapping

I Introduction
II Related Works
II-A LiDAR(-Inertial) Odometry
II-B Dynamic Data Structure in Mapping
III System Overview
IV State Estimation
IV-A Kinematic Model
IV-A1 State Transition Model
IV-A2 Measurement Model
IV-B Iterated Kalman Filter
IV-B1 Propagation
IV-B2 Residual Computation
IV-B3 Iterated Update
V Mapping
V-A Map Management
V-B Tree Structure and Construction
V-B1 Data Structure
V-B2 Construction
V-C Incremental Updates
V-C1 Point Insertion with On-tree Downsampling
V-C2 Box-wise Delete using Lazy Labels
V-C3 Attribute Update
V-D Re-balancing
V-D1 Balancing Criterion
V-D2 Re-build & Parallel Re-build
V-E K-Nearest Neighbor Search
V-F Time Complexity Analysis
V-F1 Incremental Operations
V-F2 Re-build
V-F3 K-Nearest Neighbor Search
VI Benchmark Results
VI-A Implementation
VI-B Data structure Evaluation
VI-B1 Evaluation Setup
VI-B2 Comparison Results
VI-C Accuracy Evaluation
VI-C1 RMSE Benchmark
VI-C2 Drift Benchmark
VI-D Processing Time Evaluation
VII Real-world Experiments
VII-A Platforms
VII-B Private Dataset
VII-B1 Detail Evaluation of Processing Time
VII-B2 Aggressive UAV Flight Experiment
VII-B3 Fast Motion Handheld Experiment
VII-C Outdoor Aerial Experiment
VIII Conclusion
References

Knowls

Knowl 1 — On-Manifold Iterated Kalman Filter State Estimation in FAST-LIO2
algorithm
The state estimation pipeline of FAST-LIO2 employs a tightly-coupled iterated Kalman filter operating directly on a 24-dimensional manifold $\mathcal{M} \triangleq SO(3) \times \mathbb{R}^{15} \times SO(3) \times \mathbb{R}^3$ . Upon receiving a new LiDAR scan and high-frequency IMU measurements, it compensates in-scan motion distortion via IMU-driven back-propagation, propagates the state and covariance forward, iteratively computes point-to-plane residuals against the local map represented by an ikd-Tree, computes the Kalman update using a state-dimension formulation, and updates the local map.
Input: Last optimal state estimate $\bar{x}_{k-1}$ and covariance $\bar{P}_{k-1}$ , raw LiDAR points $\{{}^L p_j\}_{j=1}^m$ in scan $k$ , IMU measurements $(a_m, \omega_m)$ over scan $k$ , convergence threshold $\epsilon$
Output: Current optimal estimate $\bar{x}_k$ and covariance $\bar{P}_k$ , transformed LiDAR points $\{{}^G \bar{p}_j\}_{j=1}^m$
1: Forward propagate state prediction $\hat{x}_k$ and covariance $\hat{P}_k$ using IMU measurements
2: Backward propagate to compensate point motion distortion to scan end-time
3: Initialize iteration index $\kappa = -1$ , state iterate $\hat{x}_k^{\kappa=0} = \hat{x}_k$
4: repeat
5: $\kappa \leftarrow \kappa + 1$
6: Compute error state transition matrix $J^\kappa$ and prior covariance $P = (J^\kappa)^{-1} \hat{P}_k (J^\kappa)^{-T}$
7: for each transformed point ${}^G \hat{p}_j = {}^G \hat{T}_{I_k}^\kappa {}^I \hat{T}_{L_k}^\kappa {}^L p_j$ do
8: Find 5 nearest neighbors in the ikd-Tree map
9: Fit plane normal ${}^G u_j$ and centroid ${}^G q_j$
10: Compute measurement residual $z_j^\kappa = {}^G u_j^T ({}^G \hat{p}_j - {}^G q_j)$ and Jacobian $H_j^\kappa$
11: end for
12: Assemble matrix $H = [H_1^{\kappa T}, \dots, H_m^{\kappa T}]^T$ , noise covariance $R = \text{diag}(R_1, \dots, R_m)$ , and residual vector $z_k^\kappa = [z_1^{\kappa T}, \dots, z_m^{\kappa T}]^T$
13: Compute Kalman gain $K = (H^T R^{-1} H + P^{-1})^{-1} H^T R^{-1}$
14: Update state iterate $\hat{x}_k^{\kappa+1} = \hat{x}_k^\kappa \boxplus (-K z_k^\kappa - (I - K H)(J^\kappa)^{-1} (\hat{x}_k^\kappa \boxminus \hat{x}_k))$
15: until $\|\hat{x}_k^{\kappa+1} \boxminus \hat{x}_k^\kappa\| < \epsilon$
16: Set $\bar{x}_k = \hat{x}_k^{\kappa+1}$ and $\bar{P}_k = (I - K H) P$
17: Transform scan points to global frame: ${}^G \bar{p}_j = {}^G \bar{T}_{I_k} {}^I \bar{T}_{L_k} {}^L p_j$ for $j = 1, \dots, m$
18: return $\bar{x}_k$ , $\bar{P}_k$ , $\{{}^G \bar{p}_j\}_{j=1}^m$
The Kalman gain formulation $K = (H^T R^{-1} H + P^{-1})^{-1} H^T R^{-1}$ requires inverting a matrix of dimension $24 \times 24$ (the dimension of the system state space), rather than an $m \times m$ matrix (the number of measurement points, where $m \gg 24$ ), yielding substantial computational savings.
Knowl 2 — Continuous and Discrete-Time LiDAR-Inertial Kinematic Models with Online Extrinsic Calibration
model/method
The state vector $x \in \mathcal{M} \triangleq SO(3) \times \mathbb{R}^{15} \times SO(3) \times \mathbb{R}^3$ (with manifold dimension $\dim(\mathcal{M}) = 24$ ) of the LiDAR-inertial system is defined by:

$x \triangleq \begin{bmatrix} {}^G R_I^T & {}^G p_I^T & {}^G v_I^T & b_\omega^T & b_a^T & {}^G g^T & {}^I R_L^T & {}^I p_L^T \end{bmatrix}^T$

where ${}^G p_I, {}^G R_I, {}^G v_I$ represent the IMU position, rotation attitude, and linear velocity in the global frame $G$ ; $b_a, b_\omega$ are the IMU accelerometer and gyroscope biases; ${}^G g$ is the gravity vector in the global frame; and ${}^I T_L = ({}^I R_L, {}^I p_L)$ is the unknown rigid extrinsic transformation from the LiDAR frame $L$ to the IMU frame $I$ .

The continuous-time kinematic model driven by raw IMU inputs $u = [\omega_m^T, a_m^T]^T$ is:

$\begin{aligned} {}^G \dot{R}_I &= {}^G R_I \lfloor \omega_m - b_\omega - n_\omega \rfloor_\wedge, & {}^G \dot{p}_I &= {}^G v_I, \\ {}^G \dot{v}_I &= {}^G R_I (a_m - b_a - n_a) + {}^G g, & \dot{b}_\omega &= n_{b\omega}, \\ \dot{b}_a &= n_{ba}, & {}^G \dot{g} &= 0_{3 \times 1}, \\ {}^I \dot{R}_L &= 0_{3 \times 3}, & {}^I \dot{p}_L &= 0_{3 \times 1} \end{aligned}$

where $\lfloor a \rfloor_\wedge$ is the $3 \times 3$ skew-symmetric cross-product matrix of $a \in \mathbb{R}^3$ , $n_a, n_\omega$ are white measurement noises of the IMU sensors, and $n_{ba}, n_{b\omega}$ are white noises driving the bias random walks.

Discretized at the IMU sampling period $\Delta t$ with process noise vector $w_i = [n_\omega^T, n_a^T, n_{b\omega}^T, n_{ba}^T]^T \sim \mathcal{N}(0, Q_i)$ , the state forward propagation follows:

$x_{i+1} = x_i \boxplus (\Delta t f(x_i, u_i, w_i))$

$\hat{x}_{i+1} = \hat{x}_i \boxplus (\Delta t f(\hat{x}_i, u_i, 0)), \quad \hat{P}_{i+1} = F_{\tilde{x}_i} \hat{P}_i F_{\tilde{x}_i}^T + F_{w_i} Q_i F_{w_i}^T$

where $F_{\tilde{x}_i} = \frac{\partial(x_{i+1} \boxminus \hat{x}_{i+1})}{\partial \tilde{x}_i} \Big|_{\tilde{x}_i=0, w_i=0}$ and $F_{w_i} = \frac{\partial(x_{i+1} \boxminus \hat{x}_{i+1})}{\partial w_i} \Big|_{\tilde{x}_i=0, w_i=0}$ .
Knowl 3 — Direct Point-to-Plane Measurement Model and Residual Formulation
equation
In direct LiDAR registration without explicit feature extraction, each raw point measurement ${}^L p_j$ (sampled at local frame $L$ and motion-compensated to the scan end-time) is corrupted by noise ${}^L n_j \sim \mathcal{N}(0, R_j)$ such that the true coordinate is ${}^L p_j^{gt} = {}^L p_j + {}^L n_j$ . When projected into the global frame using IMU pose ${}^G T_{I_k} = ({}^G R_{I_k}, {}^G p_{I_k})$ and extrinsic transformation ${}^I T_L = ({}^I R_L, {}^I p_L)$ , the point lies on a local planar map patch characterized by normal vector ${}^G u_j$ and centroid ${}^G q_j$ :

$0 = {}^G u_j^T \left( {}^G T_{I_k} {}^I T_L ({}^L p_j + {}^L n_j) - {}^G q_j \right) \triangleq h_j(x_k, {}^L p_j + {}^L n_j)$

At the $\kappa$ -th iteration of state estimate $\hat{x}_k^\kappa$ , the point is projected to ${}^G \hat{p}_j = {}^G \hat{T}_{I_k}^\kappa {}^I \hat{T}_{L_k}^\kappa {}^L p_j$ , and its 5 nearest neighbors in the global map are identified via ikd-Tree to determine ${}^G u_j$ and ${}^G q_j$ . The first-order Taylor expansion yields:

$0 = h_j(x_k, {}^L n_j) \approx z_j^\kappa + H_j^\kappa \tilde{x}_k^\kappa + v_j$

where $\tilde{x}_k^\kappa = x_k \boxminus \hat{x}_k^\kappa$ , $v_j \sim \mathcal{N}(0, R_j)$ , $H_j^\kappa$ is the Jacobian of $h_j$ with respect to $\tilde{x}_k^\kappa$ evaluated at zero, and the residual scalar $z_j^\kappa$ is given by:

$z_j^\kappa = {}^G u_j^T \left( {}^G \hat{T}_{I_k}^\kappa {}^I \hat{T}_{L_k}^\kappa {}^L p_j - {}^G q_j \right)$
Knowl 4 — Point Insertion with On-Tree Downsampling in ikd-Tree
algorithm
The ikd-Tree data structure supports simultaneous point insertion and spatial downsampling on the tree without requiring a separate pre-filtering step. Given downsample grid resolution $l$ , the point space is partitioned into cubes $C_D$ of size $l$ . For each new point $p$ , the algorithm identifies $C_D$ , queries existing points within $C_D$ , finds the point closest to the cube center $p_{center}$ , replaces all points in $C_D$ with this single representative point, and updates the tree balance.
Input: Downsample resolution $l$ , new point $p$ , parallel rebuilding switch $SW$ , tree root $RootNode$
Output: Updated tree containing downsampled point
1: $C_D \leftarrow \text{FindCube}(l, p)$
2: $p_{center} \leftarrow \text{Center}(C_D)$
3: $V \leftarrow \text{BoxwiseSearch}(RootNode, C_D)$
4: Append $p$ to $V$
5: $p_{nearest} \leftarrow \text{FindNearest}(V, p_{center})$
6: $\text{BoxwiseDelete}(RootNode, C_D, SW)$
7: $\text{Insert}(RootNode, p_{nearest}, \text{NULL}, SW)$
function Insert(T, p, father, SW)
    if T is empty then
        Initialize new leaf node $T$ with point $p$ , $T.treesize = 1$ , $T.invalidnum = 0$ , $T.deleted = \text{false}$ , $T.treedeleted = \text{false}$ , $T.range = [p, p]$ , $T.axis = (father.axis + 1) \bmod k$
    else
         $ax \leftarrow T.axis$
        if $p[ax] < T.point[ax]$ then
            Insert( $T.leftchild, p, T, SW$ )
        else
            Insert( $T.rightchild, p, T, SW$ )
        end if
        AttributeUpdate(T)
        Rebalance(T, SW)
    end if
end function
The helper AttributeUpdate(T) updates $T.treesize$ , $T.invalidnum$ , and bounding cuboid $T.range$ from its children. The time complexity of point insertion with on-tree downsampling on an ikd-Tree with $n$ nodes is $O(\log n)$ .
Knowl 5 — Box-Wise Lazy Deletion in ikd-Tree
algorithm
Box-wise deletion in ikd-Tree deletes all points contained in an axis-aligned target cuboid $C_O$ efficiently by leveraging the bounding cuboid attribute $T.range$ ( $C_T$ ) stored at each node and applying lazy deletion labels.
Input: Target deletion cuboid $C_O$ , current node $T$ , parallel rebuilding switch $SW$
Output: Updated tree with points in $C_O$ labeled as deleted
function BoxwiseDelete(T, C_O, SW)
     $C_T \leftarrow T.range$
    if $C_T \cap C_O = \emptyset$ then
        return
    end if
    if $C_T \subseteq C_O$ then
         $T.treedeleted \leftarrow \text{true}$
         $T.deleted \leftarrow \text{true}$
         $T.invalidnum \leftarrow T.treesize$
    else
         $p \leftarrow T.point$
        if $p \in C_O$ then
             $T.deleted \leftarrow \text{true}$
        end if
        if $T.leftchild \neq \text{NULL}$ then
            BoxwiseDelete( $T.leftchild, C_O, SW$ )
        end if
        if $T.rightchild \neq \text{NULL}$ then
            BoxwiseDelete( $T.rightchild, C_O, SW$ )
        end if
        AttributeUpdate(T)
        Rebalance(T, SW)
    end if
end function
When a sub-tree's bounding box $C_T$ is fully contained in $C_O$ , setting $T.treedeleted = \text{true}$ prunes recursion into all children in $O(1)$ time. Deleted nodes remain in memory until pruned during dynamic tree re-balancing.
Knowl 6 — Dynamic Re-Balancing and Parallel Double-Thread Rebuilding in ikd-Tree
algorithm
The ikd-Tree continuously checks two balancing criteria on sub-trees rooted at node $T$ :
1. $\alpha$ -balanced criterion: $\max(S(T.leftchild), S(T.rightchild)) < \alpha_{bal} (S(T) - 1)$ , with hyperparameter $\alpha_{bal} \in (0.5, 1.0)$ (default $\alpha_{bal} = 0.6$ ). This maintains the tree's maximum height at $\log_{1/\alpha_{bal}}(n)$ .
2. $\alpha$ -deleted criterion: $I(T) < \alpha_{del} S(T)$ , with hyperparameter $\alpha_{del} \in (0.0, 1.0)$ (default $\alpha_{del} = 0.5$ ), where $S(T) = T.treesize$ and $I(T) = T.invalidnum$ .
If either criterion is violated, re-balancing is triggered. Sub-trees smaller than threshold $N_{max}$ (default $N_{max} = 1500$ ) are re-built synchronously in the main thread; larger sub-trees are rebuilt asynchronously in a parallel worker thread using an operation logger to prevent locking queries.
Input: Sub-tree root node $T$ , parallel rebuilding switch $SW$ , size threshold $N_{max}$
Output: Re-balanced sub-tree
function Rebalance(T, SW)
    if ViolateCriterion(T) then
        if $T.treesize < N_{max}$ or not $SW$ then
            Rebuild(T)
        else
            ThreadSpawn(ParRebuild, T)
        end if
    end if
end function
function ParRebuild(T)
    LockUpdates(T)
     $V \leftarrow \text{Flatten}(T)$
    Unlock(T)
     $T' \leftarrow \text{Build}(V)$
    for each $op$ in OperationLogger do
        IncrementalUpdates( $T', op, \text{false}$ )
    end for
     $T_{temp} \leftarrow T$
    LockAll(T)
     $T \leftarrow T'$
    Unlock(T)
    Free( $T_{temp}$ )
end function
LockUpdates blocks incremental modifications but permits parallel $k$ NN search queries in the main thread. Modifications arriving during parallel rebuilding are queued in OperationLogger and subsequently replayed on the newly constructed balanced tree $T'$ . LockAll performs an instantaneous pointer swap.
Knowl 7 — Asymptotic Time Complexities for ikd-Tree Operations
theoretical result
For an ikd-Tree containing $n$ points in a 3-dimensional space $S_x \times S_y \times S_z$ with an operation cuboid $C_D = L_x \times L_y \times L_z$ :
1. Box-Wise Operations (Search and Lazy Deletion): The time complexity $O(H(n))$ satisfies:
$O(H(n)) = \begin{cases} O(\log n) & \text{if } \Delta_{min} > \alpha(\frac{2}{3}) \\ O(n^{1 - a - b - c}) & \text{if } \Delta_{max} \le 1 - \alpha(\frac{1}{3}) \\ O(n^{\alpha(1/3) - \Delta_{min} - \Delta_{med}}) & \text{if neither applies and } \Delta_{med} < \alpha(\frac{1}{3}) - \alpha(\frac{2}{3}) \\ O(n^{\alpha(2/3) - \Delta_{min}}) & \text{otherwise} \end{cases}$

where $a = \log_n \frac{S_x}{L_x}$ , $b = \log_n \frac{S_y}{L_y}$ , $c = \log_n \frac{S_z}{L_z}$ , $\Delta_{min}, \Delta_{med}, \Delta_{max}$ are the sorted values of $a, b, c$ , and $\alpha(u)$ is the Flajolet-Puech function ( $\\alpha(1/3) \approx 0.7162$ , $\alpha(2/3) \approx 0.3949$ ).
1. Point Insertion with On-Tree Downsampling: $O(\log n)$ time complexity, as downsample search/delete operates over small bounding cubes where $\Delta_{min} > \alpha(2/3)$ , and tree insertion on a tree of bounded height $\log_{1/\alpha_{bal}}(n)$ is $O(\log n)$ .
2. $k$ -Nearest Neighbor ( $k$ NN) Search: $O(\log n)$ expected time complexity, due to the bounded height $\log_{1/\alpha_{bal}}(n)$ and constant expected backtracking steps $\bar{l}$ .
3. Tree Re-balancing: $O(n \log n)$ time complexity for single-thread rebuilding; $O(n)$ time complexity from the perspective of the main thread during double-thread parallel rebuilding (main thread only performs $O(n)$ flattening and $O(1)$ pointer update).
Knowl 8 — Bounded Local Map Region Management via Moving Cuboids
model/method
To prevent map size and memory from growing unboundedly during long-term navigation, FAST-LIO2 maintains points within a local cubic region of side length $L$ (default $L = 1000\text{ m}$ ) centered around the current LiDAR position.

The LiDAR detection range is modeled as a sphere of radius $r = \gamma R$ centered at the current estimated position $p$ , where $R$ is the LiDAR maximum FoV measuring range and $\gamma > 1$ is a relaxation factor. When the sensor moves to a new position $p'$ such that the detection sphere touches any boundary of the local map cube, the map cube is translated along that boundary direction by a fixed distance $d = (\gamma - 1)R$ .

All historical map points situated in the subtraction volume (the geometric region present in the previous cube but outside the translated cube) are removed from the ikd-Tree via a single box-wise deletion operation. This keeps the active map points bounded without triggering full map rebuilds.

Knowl 9 — Benchmark Accuracy and Runtime Comparison of FAST-LIO2 against State-of-the-Art LIO Methods

data/table

FAST-LIO2 was evaluated across 19 benchmark sequences from 5 datasets (lili [solid-state Livox Horizon], utbm [32-line Velodyne], ulhk [32-line Velodyne], nclt [32-line Velodyne], and liosam [16-line Velodyne]) on an Intel i7-8550U processor (DJI Manifold 2-C) and Khadas VIM3 (ARM Cortex-A73). The methods compared were LILI-OM, LIO-SAM, LINS, and an ablated feature-based variant of FAST-LIO2.

Sequence	FAST-LIO2	FAST-LIO2	FAST-LIO2	LILI-OM	LIO-SAM	LINS	FAST-LIO2	LIO-SAM
	(1000m) RMSE	(Feature) RMSE	(ARM) Total	Total Time	Total Time	Total Time	Total Time	Total Time
	[m]	[m]	Time [ms]	[ms]	[ms]	[ms]	[ms]	[ms]
utbm_8	27.29	27.21	100.00	150.05	—	191.36	22.05	—
utbm_9	51.60	53.81	91.05	166.84	—	192.88	25.44	—
utbm_10	16.80	22.59	94.62	163.39	—	199.73	22.48	—
ulhk_4	2.57	2.61	91.12	127.20	134.79	128.42	20.14	134.79
nclt_4	8.71	8.50	69.09	160.95	197.41	234.83	15.72	197.41
nclt_5	6.68	7.82	68.95	150.98	203.55	246.71	16.60	203.55
nclt_6	20.96	20.57	66.64	209.35	Fail	249.79	15.84	Fail
nclt_7	6.58	6.77	70.24	149.34	240.68	254.65	16.87	240.68
nclt_8	30.08	31.17	57.03	111.08	179.39	198.48	14.25	179.39
nclt_9	5.56	6.09	54.82	111.70	131.14	195.57	13.65	131.14
nclt_10	16.29	16.61	89.65	213.88	347.75	335.80	21.79	347.75
liosam_1	4.58	7.85	60.60	132.73	148.86	203.57	14.77	148.86

FAST-LIO2 achieved the lowest translation RMSE in 18 out of 19 evaluated benchmark sequences. Its overall computation time per scan on the Intel platform averaged 11–31 ms, running roughly $8\times$ faster than LILI-OM, $10\times$ faster than LIO-SAM, and $6\times$ faster than LINS, while executing odometry and mapping at 10–18 Hz on the low-power ARM platform.

Knowl 10 — Runtime Comparison of ikd-Tree against Octree, R*-Tree, and nanoflann for Dynamic Point Cloud Mapping

data/table

The performance of ikd-Tree was evaluated within FAST-LIO2 against three state-of-the-art dynamic spatial data structures across 18 dataset sequences: Boost Geometry $R^*$ -tree, PCL Octree, and nanoflann dynamic $k$ -d tree. The metrics measured average time per scan for incremental update (point insertion with downsampling and box-wise delete), single-thread 5-nearest-neighbor ( $k$ NN) search, and overall processing time.

Dataset	Incremental Update [ms]				kNN Search [ms]				Total [ms]
	ikd-Tree	nanoflann	Octree	R*-tree	ikd-Tree	nanoflann	Octree	R*-tree	ikd-Tree	nanoflann	Octree	R*-tree
utbm_1	3.23	3.43	2.12	3.94	15.19	15.80	42.88	22.56	18.42	19.22	45.00	26.50
utbm_3	3.77	4.17	2.36	4.52	16.83	18.54	45.72	23.12	20.60	22.70	48.08	27.64
utbm_7	3.82	4.62	2.55	5.26	15.42	16.97	42.06	25.87	19.24	21.59	44.61	31.13
ulhk_1	1.97	1.87	1.12	2.30	18.23	21.73	48.30	23.45	20.20	23.60	49.43	25.75
ulhk_2	3.51	3.43	2.32	4.23	22.26	26.07	64.56	31.75	25.77	29.49	66.88	35.98
nclt_1	1.14	1.59	0.99	2.07	14.50	18.83	41.58	28.07	15.64	20.41	42.57	30.14
nclt_2	1.35	2.04	1.36	2.66	14.68	18.99	46.56	29.20	16.03	21.03	47.91	31.86
nclt_3	1.00	1.42	1.03	2.20	14.41	19.25	46.19	30.10	15.42	20.67	47.22	32.29
lili_1	1.41	1.42	0.83	1.79	9.20	9.71	26.31	12.65	10.61	11.13	27.15	14.44
lili_3	1.10	1.14	0.63	1.38	8.46	8.87	25.45	13.18	9.57	10.00	26.08	14.56
lili_5	1.22	1.28	0.80	1.63	10.23	11.34	33.53	12.78	11.45	12.62	34.33	14.41

While Octree achieved the lowest incremental update time, its $k$ NN search latency was $2.5\times$ to $3.5\times$ slower than ikd-Tree. Although nanoflann dynamic $k$ -d tree achieved comparable average update speed, its lack of physical point deletion caused its tree size to grow past $6 \times 10^6$ points on utbm and $10^7$ on nclt, triggering incremental update latency spikes exceeding 3 seconds on utbm and 7 seconds on nclt. In contrast, ikd-Tree kept its maximum update time under 215 ms across all sequences while delivering the lowest total per-scan processing time.

Coverage note — Omitted qualitative visual trajectory demonstrations, specific hardware mechanical CAD assembly details, and descriptions of previous baseline methods (LOAM, BALM, LILI-OM feature extractors) that are not original contributions of FAST-LIO2.

References

1.C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, “Svo: Semidirect visual odometry for monocular and multicamera systems,” IEEE Transactions on Robotics, vol. 33, no. 2, pp. 249–265, 2016.
2.C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
3.T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
4.C. Campos, R. Elvira, J. J. G. Rodrıguez, J. M. Montiel, and J. D. Tardos, ´ “Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam,” IEEE Transactions on Robotics, 2021.
5.R. Newcombe, “Dense visual slam,” Ph.D. dissertation, Imperial College London, 2012.
6.M. Meilland, C. Barat, and A. Comport, “3d high dynamic range dense visual slam and its application to real-time object re-lighting,” in 2013 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE, 2013, pp. 143–152.
7.M. Bloesch, J. Czarnowski, R. Clark, S. Leutenegger, and A. J. Davison, “Codeslam—learning a compact, optimisable representation for dense visual slam,” in Proceedings of the IEEE conference on computer vision and pattern recognition, 2018, pp. 2560–2568.
8.C. Kerl, J. Sturm, and D. Cremers, “Dense visual slam for rgb-d cameras,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013, pp. 2100–2106.
9.S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron, J. Diebel, P. Fong, J. Gale, M. Halpenny, G. Hoffmann, et al., “Stanley: The robot that won the darpa grand challenge,” Journal of field Robotics, vol. 23, no. 9, pp. 661–692, 2006.
10.C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer, et al., “Autonomous driving in urban environments: Boss and the urban challenge,” Journal of Field Robotics, vol. 25, no. 8, pp. 425–466, 2008.
11.J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel, J. Z. Kolter, D. Langer, O. Pink, V. Pratt, et al., “Towards fully autonomous driving: Systems and algorithms,” in 2011 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2011, pp. 163–168.
12.S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1688–1695, 2017.
13.F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, vol. 36, no. 4, pp. 710–733, 2019.
14.D. Wang, C. Watkins, and H. Xie, “Mems mirrors for lidar: A review,” Micromachines, vol. 11, no. 5, p. 456, 2020.
15.Z. Liu, F. Zhang, and X. Hong, “Low-cost retina-like robotic lidars based on incommensurable scanning,” IEEE/ASME Transactions on Mechatronics, pp. 1–1, 2021.
16.J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3126–3131.
17.K. Li, M. Li, and U. D. Hanebeck, “Towards high-performance solid-state-lidar-inertial odometry and mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 5167–5174, 2021.
18.J. Lin and F. Zhang, “A fast, complete, point cloud based loop closure for lidar odometry and mapping,” arXiv preprint arXiv:1909.11811, 2019.
19.H. Wang, C. Wang, and L. Xie, “Lightweight 3-d localization and mapping for solid-state lidar,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1801–1807, 2021.
20.Z. Liu and F. Zhang, “Balm: Bundle adjustment for lidar mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184–3191, 2021.
21.C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 15–22.
22.W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, pp. 1–1, 2021.
23.J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
24.G. C. Sharp, S. W. Lee, and D. K. Wehe, “Icp registration using invariant features,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 24, no. 1, pp. 90–102, 2002.
25.K.-L. Low, “Linear least-squares optimization for point-to-plane icp surface registration,” Chapel Hill, University of North Carolina, vol. 4, no. 10, pp. 1–3, 2004.
26.A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics: science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435.
27.T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758–4765.
28.A. Tagliabue, J. Tordesillas, X. Cai, A. Santamaria-Navarro, J. P. How, L. Carlone, and A.-a. Agha-mohammadi, “Lion: Lidar-inertial observability-aware navigator for vision-denied environments,” arXiv preprint arXiv:2102.03443, 2021.
29.H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometry and mapping,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 3144–3150.
30.T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020, pp. 5135–5142.
31.C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “Lins: A lidar-inertial state estimator for robust and efficient navigation,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 8899–8906.
32.M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012.
33.K. Koide, M. Yokozuka, S. Oishi, and A. Banno, “Voxelized gicp for fast and accurate 3d point cloud registration,” EasyChair Preprint, no. 2703, 2020.
34.P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” in Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453), vol. 3. IEEE, 2003, pp. 2743–2748.
35.M. Magnusson, “The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection,” Ph.D. dissertation, Örebro universitet, 2009.
36.M. Magnusson, A. Nuchter, C. Lorken, A. J. Lilienthal, and J. Hertzberg, “Evaluation of 3d registration reliability and speed-a comparison of icp and ndt,” in 2009 IEEE International Conference on Robotics and Automation. IEEE, 2009, pp. 3907–3912.
37.A. Guttman, “R-trees: A dynamic index structure for spatial searching,” in Proceedings of the 1984 ACM SIGMOD international conference on Management of data, 1984, pp. 47–57.
38.N. Beckmann, H.-P. Kriegel, R. Schneider, and B. Seeger, “The r*-tree: An efficient and robust access method for points and rectangles,” in Proceedings of the 1990 ACM SIGMOD international conference on Management of data, 1990, pp. 322–331.
39.D. Meagher, “Geometric modeling using octree encoding,” Computer graphics and image processing, vol. 19, no. 2, pp. 129–147, 1982.
40.J. L. Bentley, “Multidimensional binary search trees used for associative searching,” Communications of the ACM, vol. 18, no. 9, pp. 509–517, 1975.
41.J. H. Friedman, J. L. Bentley, and R. A. Finkel, “an algorithm for finding best matches in logarithmic expected time,” ACM Transactions on Mathematical Software (TOMS), vol. 3, no. 3, pp. 209–226, 1977.
42.J. L. Vermeulen, A. Hillebrand, and R. Geraerts, “A comparative study of k-nearest neighbour techniques in crowd simulation,” Computer Animation and Virtual Worlds, vol. 28, no. 3-4, p. e1775, 2017.
43.J. Elseberg, S. Magnenat, R. Siegwart, and A. Nüchter, “Comparison of nearest-neighbor-search strategies and implementations for efficient shape registration,” Journal of Software Engineering for Robotics, vol. 3, no. 1, pp. 2–12, 2012.
44.S. Arya and D. Mount, “Ann: library for approximate nearest neighbor searching,” in Proceedings of IEEE CGC Workshop on Computational Geometry, Providence, RI, 1998.
45.M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration.” VISAPP (1), vol. 2, no. 331-340, p. 2, 2009.
46.W. Hunt, W. R. Mark, and G. Stoll, “Fast kd-tree construction with an adaptive error-bounded heuristic,” in 2006 IEEE Symposium on Interactive Ray Tracing. IEEE, 2006, pp. 81–88.
47.S. Popov, J. Gunther, H.-P. Seidel, and P. Slusallek, “Experiences with streaming construction of sah kd-trees,” in 2006 IEEE Symposium on Interactive Ray Tracing. IEEE, 2006, pp. 89–94.
48.M. Shevtsov, A. Soupikov, and A. Kapustin, “Highly parallel fast kd-tree construction for interactive ray tracing of dynamic scenes,” Computer Graphics Forum, vol. 26, no. 3, pp. 395–404, 2007.
49.K. Zhou, Q. Hou, R. Wang, and B. Guo, “Real-time kd-tree construction on graphics hardware,” ACM Transactions on Graphics, vol. 27, no. 5, pp. 1–11, 2008.
50.I. Galperin and R. L. Rivest, “Scapegoat trees.” in SODA, vol. 93, 1993, pp. 165–174.
51.J. L. Bentley and J. B. Saxe, “Decomposable searching problems i: Static-to-dynamic transformation,” J. algorithms, vol. 1, no. 4, pp. 301–358, 1980.
52.M. H. Overmars, The design of dynamic data structures. Springer Science & Business Media, 1987, vol. 156.
53.O. Procopiuc, P. K. Agarwal, L. Arge, and J. S. Vitter, “Bkd-tree: A dynamic scalable kd-tree,” in International Symposium on Spatial and Temporal Databases. Springer, 2003, pp. 46–65.
54.J. L. Blanco and P. K. Rai, “nanoflann: a C++ header-only fork of FLANN, a library for nearest neighbor (NN) with kd-trees,” https://github.com/jlblancoc/nanoflann(v1.3.2), 2014.
55.D. He, W. Xu, and F. Zhang, “Embedding manifold structures into kalman filters,” arXiv preprint arXiv:2102.03804, 2021.
56.P. Chanzy, L. Devroye, and C. Zamora-Cura, “Analysis of range search for random kd trees,” Acta informatica, vol. 37, no. 4-5, pp. 355–383, 2001.
57.Z. Yan, L. Sun, T. Krajnik, and Y. Ruichek, “EU long-term dataset with multiple sensors for autonomous driving,” in Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
58.W. Wen, Y. Zhou, G. Zhang, S. Fahandezh-Saadi, X. Bai, W. Zhan, M. Tomizuka, and L.-T. Hsu, “Urbanloco: a full sensor suite dataset for mapping and localization in urban scenes,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 2310–2316.
59.N. Carlevaris-Bianco, A. K. Ushani, and R. M. Eustice, “University of michigan north campus long-term vision and lidar dataset,” The International Journal of Robotics Research, vol. 35, no. 9, pp. 1023–1035, 2016.
60.G. Barend, L. Bruno, L. Mateusz, W. Adam, K. Menelaos, and F. Vissarion, “Boost geometry library,” https://www.boost.org/doc/libs/1_65_1/libs/geometry/, September 2017.
61.R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),” in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 1–4.
62.G. Lu, W. Xu, and F. Zhang, “Model predictive control for trajectory tracking on differentiable manifolds,” arXiv preprint arXiv:2106.15233, 2021.
63.F. Kong, W. Xu, and F. Zhang, “Avoiding dynamic small obstacles with onboard sensing and computating on aerial robots,” arXiv preprint arXiv:2103.00406, 2021.

Citation

MLA

Xu, W., et al. “FAST-LIO2: Fast Direct LiDAR-inertial Odometry”. arXiv, 2021, http://arxiv.org/abs/2107.06829v1.

APA

Xu, W., Cai, Y., He, D., Lin, J., & Zhang, F. (2021). FAST-LIO2: Fast Direct LiDAR-inertial Odometry. arXiv. http://arxiv.org/abs/2107.06829v1

Chicago

Xu, W., Y. Cai, D. He, J. Lin, and F. Zhang. 2021. “FAST-LIO2: Fast Direct LiDAR-inertial Odometry”. arXiv. http://arxiv.org/abs/2107.06829v1.

Harvard

Xu, W. et al. (2021) “FAST-LIO2: Fast Direct LiDAR-inertial Odometry”, arXiv [Preprint]. Available at: http://arxiv.org/abs/2107.06829v1.

Vancouver

1. Xu W, Cai Y, He D, Lin J, Zhang F (2021) FAST-LIO2: Fast Direct LiDAR-inertial Odometry. arXiv

BibTeX

@article{xu2021fast,
  title = {FAST-LIO2: Fast Direct LiDAR-inertial Odometry},
  author = {Xu, Wei and Cai, Yixi and He, Dongjiao and Lin, Jiarong and Zhang, Fu},
  year = {2021},
  journal = {arXiv},
  url = {http://arxiv.org/abs/2107.06829v1},
  eprint = {2107.06829}
}

Metadata:arXiv

Source Code

This paper has an official code repository available. Click below to access the source code.

View Repository

Access the Paper

This paper is available from its original source. Click below to access the PDF.

Open PDF

FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

Abstract

Topics

Keywords

Table of Contents

Knowls

Knowl 1 — On-Manifold Iterated Kalman Filter State Estimation in FAST-LIO2

Knowl 2 — Continuous and Discrete-Time LiDAR-Inertial Kinematic Models with Online Extrinsic Calibration

Knowl 3 — Direct Point-to-Plane Measurement Model and Residual Formulation

Knowl 4 — Point Insertion with On-Tree Downsampling in ikd-Tree

Knowl 5 — Box-Wise Lazy Deletion in ikd-Tree

Knowl 6 — Dynamic Re-Balancing and Parallel Double-Thread Rebuilding in ikd-Tree

Knowl 7 — Asymptotic Time Complexities for ikd-Tree Operations