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.
Wei Xu, Yixi Cai,
Dongjiao He,
Jiarong Lin,
Fu Zhang
