326 IEEE TRANSACTIONS ON ROBOTICS, VOL.

41, 2025

FAST-LIVO2: Fast, Direct LiDAR–Inertial–Visual

Odometry
Chunran Zheng , Student Member, IEEE, Wei Xu , Zuhao Zou , Tong Hua , Chongjian Yuan ,
Dongjiao He , Member, IEEE, Bingyang Zhou, Member, IEEE, Zheng Liu , Member, IEEE,
Jiarong Lin , Member, IEEE, Fangcheng Zhu , Student Member, IEEE, Yunfan Ren , Student Member, IEEE,
Rong Wang, Fanle Meng, and Fu Zhang , Member, IEEE

Abstract—This paper presents FAST-LIVO2, a fast and direct unknown environments. Due to its ability to estimate poses and
LiDAR-inertial-visual odometry framework designed for accurate reconstruct maps in real time, SLAM has become indispensable
and robust state estimation in SLAM tasks, enabling real-time for various robot navigation tasks. The localization process
robotic applications. FAST-LIVO2 integrates IMU, LiDAR, and delivers crucial state feedback for the robot’s onboard con-
image data through an efficient error-state iterated Kalman filter trollers, while the dense 3-D map provides key environmental
(ESIKF). To address the dimensional mismatch between LiDAR
and image measurements, we adopt a sequential update strategy.
information, such as free spaces and obstacles, essential for
Efficiency is further enhanced using direct methods for LiDAR effective trajectory planning. A colored map also carries sub-
and visual data fusion: the LiDAR module registers raw points stantial semantic information, enabling a vivid representation of
without extracting features, while the visual module minimizes pho- the real world that opens up vast potential applications, such as
tometric errors without relying on feature extraction. Both LiDAR virtual and augmented reality, 3-D modeling, and robot–human
and visual measurements are fused into a unified voxel map. The interactions.
LiDAR module constructs the geometric structure, while the visual Currently, several SLAM frameworks have been successfully
module links image patches to LiDAR points, enabling precise implemented with single-measurement sensors, primarily cam-
image alignment. Plane priors from LiDAR points improve align- eras [1], [2], [3], [4], or LiDAR [5], [6], [7]. Although visual
ment accuracy and are refined dynamically during the process. and LiDAR SLAM have shown promise in their own domains,
Additionally, an on-demand raycast operation and real-time image
exposure estimation enhance robustness. Extensive experiments on
each has inherent limitations that constrain their performance in
benchmark and custom datasets demonstrate that FAST-LIVO2 various scenarios.
outperforms state-of-the-art systems in accuracy, robustness, and Visual SLAM, leveraging cost-effective CMOS sensors and
efficiency. Key modules are validated, and we showcase three lenses, is capable of establishing accurate data associations,
applications: UAV navigation highlighting real-time capabilities, thereby achieving a certain level of localization accuracy. The
airborne mapping demonstrating high accuracy, and 3D model abundance of color information further enriches the semantic
rendering (mesh-based and NeRF-based) showcasing suitability for perception. Further leveraging this enhanced scene comprehen-
dense mapping. Code and datasets are open-sourced on GitHub to sion; deep learning methods are employed for robust feature
benefit the robotics community. extraction and dynamic object filtering. However, the lack of di-
Index Terms—3-D reconstruction, aerial navigation, sensor rect depth measurement in visual SLAM necessitates concurrent
fusion, simultaneous localization and mapping (SLAM). optimization of map points via operations, such as triangulation
or depth filtering, which introduces significant computational
overhead that often limits map accuracy and density. Visual
I. INTRODUCTION SLAM also encounters numerous other limitations, such as
N RECENT years, simultaneous localization and mapping varying measurement noise across different scales, sensitivity
I (SLAM) technology1 has seen significant advancements,
particularly in real-time 3-D reconstruction and localization in
to illumination changes, and the impact of texture-less environ-
ments on data association.
LiDAR SLAM, utilizing LiDAR sensors, obtains precise
depth measurements directly, offering superior precision and
Received 24 April 2024; revised 23 August 2024; accepted 1 October 2024.
Date of publication 19 November 2024; date of current version 12 December
efficiency in localization and mapping tasks compared with
2024. This work was supported in part by the CETC ISA and in part by the Hong visual SLAM. Despite these strengths, LiDAR SLAM exhibits
Kong General Research Fund (GRF) with under Grant 17206421. This article several significant shortcomings. On one hand, the point cloud
was recommended for publication by Associate Editor A. Nuechter and Editor maps it reconstructs, albeit detailed, lack color information,
J. Civera upon evaluation of the reviewers’ comments. (Corresponding author: thereby reducing their information scale. On the other hand,
Fu Zhang.)
Chunran Zheng, Wei Xu, Zuhao Zou, Tong Hua, Chongjian Yuan, Dongjiao LiDAR SLAM performance tends to deteriorate in environments
He, Bingyang Zhou, Zheng Liu, Jiarong Lin, Fangcheng Zhu, Yunfan Ren, and presenting insufficient geometric constraints, such as narrow
Fu Zhang are with the Mechatronics and Robotic Systems (MaRS) Laboratory, tunnels and a single and extended wall.
Department of Mechanical Engineering, University of Hong Kong, Hong Kong, As the demand to operate intelligent robots in the real world
SAR 999077, China (e-mail: fuzhang@[Link]).
Rong Wang and Fanle Meng are with the Information Science Academy,
grows, especially in environments that often lack structure or
China Electronics Technology Group Corporation, Beijing 100846, China. texture, it is becoming clear that existing systems relying on
Digital Object Identifier 10.1109/TRO.2024.3502198 a single sensor cannot provide the accurate and robust pose
1 [Link]
estimation as required. To address this issue, the fusion of
1941-0468 © 2024 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See [Link] for more information.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 327

commonly used sensors, such as LiDAR, camera, and IMU, is FAST-LIVO2 is developed based on FAST-LIVO first pro-
gaining increasing attention. This strategy not only combines the posed in our previous work [8]. The new contributions compared
strengths of these sensors to provide enhanced pose estimation, to FAST-LIVO are listed as follows.
but also aids in the construction of accurate, dense, and colored 1) We propose an efficient ESIKF framework with sequential
point cloud maps, even in environments where the performance update to address the dimension mismatch between Li-
of individual sensors degenerate. DAR and visual measurements, improving the robustness
Efficient and accurate LiDAR–inertial–visual odometry of FAST-LIVO that uses asynchronous updates.
(LIVO) and mapping are still challenging problems. 2) We use (and even refine) plane priors from LiDAR points
1) The entire LIVO system is tasked with processing LiDAR for improved accuracy. In contrast, FAST-LIVO assumes
measurements, consisting of hundreds to thousands of all pixels in a patch share the same depth, a wild assump-
points per second, as well as high-rate, high-resolution tion significantly reducing the accuracy of affine warping
images. The challenge of fully utilizing such a vast amount in image alignment.
of data, particularly with limited onboard resources, ne- 3) We propose a reference patch update strategy to im-
cessitates exceptional computational efficiency. prove the accuracy of image alignment, by selecting high-
2) Many existing systems typically incorporate a LiDAR– quality, inlier reference patches that have large parallax
inertial odometry (LIO) subsystem and a visual–inertial and sufficient texture details. FAST-LIVO selects the ref-
odometry (VIO) subsystem, each necessitating the extrac- erence patch based on proximity to the current view, often
tion of features from visual and LiDAR data respectively resulting in low-quality reference patches degrading the
to reduce computational load. In environments that lack accuracy.
structure or texture, this extraction process often results 4) We conduct online exposure time estimation for handling
in limited feature points. Furthermore, to optimize feature environment illumination variation. FAST-LIVO did not
extraction, extensive engineering adaptations are essen- address this issue, leading to poor convergence in image
tial to accommodate the variability in LiDAR scanning alignment under significant lighting changes.
patterns and point densities. 5) We propose on-demand voxel raycasting to enhance the
3) To reduce computational demands and achieve tighter system robustness in the absence of LiDAR point mea-
integration between camera and LiDAR measurements, surements caused by LiDAR close proximity blind zones,
a unified map is essential to manage sparse points and the an issue not considered in FAST-LIVO.
observed high-resolution image measurements simultane- Each of the above-mentioned contributions are evaluated in
ously. However, designing and maintaining such maps are comprehensive ablation studies to verify their effectiveness.
particularly challenging considering the heterogeneous We implement the proposed system as practical open software,
measurements of LiDAR and cameras. meticulously optimized for real-time operation on both Intel and
4) To ensure the accuracy of the reconstructed colored point ARM processors. The system is versatile, supporting multiline
cloud, pose estimation needs to achieve pixel-level accu- spinning LiDARs, emerging solid-state LiDARs with uncon-
racy. Meeting this standard presents considerable chal- ventional scanning patterns, as well as both pinhole cameras
lenges: proper hardware synchronization, rigorous pre- and various fisheye cameras.
calibration of extrinsic parameters between LiDAR and Besides, we conduct extensive experiments on 25 sequences
cameras, precise recovery of exposure time, and a fusion of public datasets (i.e., Hilti and NTU-VIRAL datasets),
strategy capable of reaching pixel-level accuracy in real alongside various representative private datasets, enabling a
time. comparison with other state-of-the-art (SOTA) SLAM systems
Motivated by these issues, we propose FAST-LIVO2, a high- (e.g., R3LIVE, LVI-SAM, FAST-LIO2, etc). Both qualitative
efficiency LIVO system that tightly integrates LiDAR, image and quantitative results demonstrate that our proposed system
and IMU measurements through a sequentially updated error- significantly outpaces other counterparts in terms of accuracy
state iterated Kalman filter (ESIKF). With the prior from IMU and robustness at a reduced computation cost.
propagation, the system state is updated sequentially, first by the Taking a step further to underline the real-world applicabil-
LiDAR measurements and then by the image measurements, ity and versatility of our system, we deploy three distinctive
both utilizing direct methods based on a single unified voxel applications. First, fully onboard autonomous UAV naviga-
map. Specifically, in the LiDAR update, the system registers raw tion, demonstrating the system’s real-time capabilities, marks
points to the map to construct and update its geometric structure, a pioneering instance of employing a LiDAR–inertial–visual
and in the visual update, the system reuses LiDAR map points as system for real-world autonomous UAV flights. Second, air-
the visual map points directly without extracting, triangulating, borne mapping showcases the system’s pixel-level precision
or optimizing any visual features from images. The chosen visual under structure-less environments in practical use. Finally, the
map points in the map are attached with reference image patches high-quality generation of mesh, texturing, and NeRF models
previously observed and then projected to the current image to underscores the system’s suitability for rendering tasks. We
align its pose by minimizing the direct photometric errors (i.e., make our code and dataset available on GitHub.
sparse image alignment). To improve the accuracy in the image
alignment, FAST-LIVO2 dynamically updates the reference
patches and uses the plane priors obtained from LiDAR points. II. RELATED WORKS
For improved computation efficiency, FAST-LIVO2 uses
LiDAR points to identify visual map points visible from the A. Direct Methods
current image and conduct an on-demanding voxel raycast Direct methods stand out as a prominent approach for fast
in case of no LiDAR points. FAST-LIVO2 also estimates the pose estimation in both visual and LiDAR SLAM. Unlike
exposure time in real time to handle illumination variation. feature-based methods [5], [6], [9], [10] which necessitate the

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
328 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

extraction of salient feature points (e.g., corners and edge pixels measurement-level tight coupling, they remain loosely coupled
in images; plane and edge points in LiDAR scans) and the gener- in state estimation, primarily due to the absence of constraints
ation of robust descriptors for matching, direct methods directly directly derived from LiDAR measurements at state estimation.
leverage raw measurements to optimize the sensor pose [11] Another issue arises as 3-D LiDAR points do not have a one-
by minimizing an error function based on photometric error or to-one correspondence with 2-D image feature points and/or
point-to-plane residuals, e.g., [3], [12], [13], [14]. By eliminat- lines due to mismatched resolutions. This mismatch requires
ing the time-consuming feature extraction and matching, direct interpolation in depth association, introducing potential errors.
methods offer fast pose estimation. Nonetheless, the absence of To address this, DVL-SLAM [28] employs a direct method for
feature matching requires fairly accurate state prior estimation visual tracking, wherein the LiDAR points are directly projected
to avoid local minima. into the image to ascertain the depth of corresponding pixel
Direct methods in visual SLAM can be broadly categorized positions.
into dense direct, semidense direct, and sparse direct methods. The works mentioned previously have not achieved tight
Dense direct methods, predominantly adopted for RGB-D cam- coupling at the state estimation level. In pursuit of higher
eras with full depth measurements as exemplified by authors accuracy and robustness, many recent studies have emerged
in [15], [16], and [17], apply image-to-model alignment for that jointly optimize sensor data in a tightly coupled man-
pose estimation. In contrast, semidense direct methods [3], [18] ner. To name a few, LIC-Fusion [29] tightly fuses IMU mea-
implement direct image alignment by capitalizing on pixels surements, sparse visual features, and LiDAR plane and edge
with significant gray-level gradients for estimation. Sparse direct features based on the MSCKF [30] framework. The subse-
methods [2], [12] focus on delivering accurate state estimation quent LIC-Fusion2.0 [31] enhances LiDAR pose estimation by
through only a few well-selected raw patches, thus further di- implementing plane-feature tracking within a sliding window.
minishing the computational burden in comparison to both dense VILENS [32] offers a joint optimization of visual, LiDAR, and
and semidense direct methods. inertial data through a unified factor graph, relying on fixed
Unlike direct visual SLAM methods, direct LiDAR SLAM lag smoothing. R2LIVE [33] tightly fuses the LiDAR, camera,
systems [13], [14], [19], [20] do not distinguish between dense and IMU measurements in an on-manifold iterated Kalman
and sparse approaches and commonly use spatially downsam- filter [34]. For the VIO subsystem in R2LIVE, a sliding window
pled or temporally downsampled raw points in each scan to optimization is used to triangulate the locations of visual features
construct constraints for pose optimization. in the map.
In our work, we harness the principles of the direct method Several systems achieve complete tight coupling at both the
for both LiDAR and visual modules. The LiDAR module of our measurement and state estimation levels. LVI-SAM [35] fuses
system is adapted from VoxelMap [14], and the visual model is the LiDAR, visual, and inertial sensors in a tightly coupled
based on a variant of sparse direct method [12]. While drawing smoothing and mapping framework, which is built atop a factor
inspiration from sparse direct image alignment in [12], our visual graph. The VIO subsystem performs visual feature tracking
module differs by reutilizing the LiDAR points as visual map and extracts feature depth using LiDAR scans. R3LIVE [36]
points, thus mitigating the intensive backend computations (i.e., constructs the geometric structure of the global map by LIO and
feature alignment, sliding window optimization and/or depth renders map texture by VIO. These two subsystems estimate
filtering). the system state jointly by fusing their respective LiDAR or
visual data with IMUs. The advanced version, R3LIVE++ [37],
estimates exposure time in real time and conducts photometric
B. LiDAR–Visual(–Inertial) SLAM calibration in advance [38], which enables the system to recover
The incorporation of multiple sensors in LiDAR–visual– the radiance of map points. Unlike most previously mentioned
inertial SLAM equips the system with the capability to handle a LiDAR-inertial-visual systems that rely on feature-based meth-
wide range of challenging environments, particularly when one ods for both LIO and VIO subsystems, R3LIVE series [36],
sensor experiences failure or partial degeneration. Motivated by [37] adopt direct methods for both without feature extraction,
this, the research community has seen the emergence of various enabling them to capture subtle environmental features even in
LiDAR–visual–inertial SLAM systems. Existing methods can texture-less or structure-less scenarios.
generally be divided into two categories: loosely coupled and Our system also jointly estimates the state using LiDAR,
tightly coupled. The classification can be determined from two image and IMU data, and maintains a tightly coupled voxel
perspectives: the state estimation level and the raw measurement map at the measurement level. Furthermore, our system uses
level. At the state estimation level, the key is whether the direct methods, harnessing raw LiDAR points for LiDAR scan
estimate from one sensor serves as an optimization objective registration and employing raw image patches for visual track-
in another sensor’s model. At the raw measurement level, it ing. The key difference between our system and R3LIVE (or
involves whether raw data from different sensors are combined. R3LIVE++) is that R3LIVE (and R3LIVE++) operate at an indi-
Zhang et al. [21] proposed a LiDAR–visual–inertial SLAM vidual pixel level in the VIO, while our system operates at image
system that is loosely coupled at the state estimation level. In patch levels. This difference bestows our system with marked
this system, VIO subsystem only provides the initial pose for the advantages. First, in terms of robustness, our methodology uses
scan registration in LIO subsystem, instead of being optimized a simplified, one-step frame-to-map sparse image alignment for
jointly with the scan registration. VIL-SLAM [22] employs a pose estimation, mitigating the heavy reliance on an accurate ini-
similar loosely coupled method, not utilizing joint optimization tial state that has to be obtained by a frame-to-frame optical flow
of LiDAR, camera, and IMU measurements. in R3LIVEs. Consequently, our system simplifies and improves
Some systems (e.g., DEMO [23], LIMO [24], CamVox [25], upon the two-stage frame-to-frame and frame-to-map operations
[26]) use 3-D LiDAR points to provide depth measurements for in R3LIVE. Second, from a computational standpoint, the VIO
the visual module [1], [4], [27]. While these systems exhibit in R3LIVE predominantly employs a dense direct method which

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 329

Fig. 1. FAST-LIVO2 mapping results generated in real time. (a)–(c) showcase airborne mapping, (d) represents a retail street collected with a handheld device,
and (e) demonstrates an experiment where a UAV carrying a LiDAR, camera, and inertial sensor perform real-time state estimation (i.e., FAST-LIVO2), trajectory
planning, and tracking control all on its onboard computer. In (d)–(e), blue lines represent the computed trajectory. In (e1)–(e4), white points indicate the LiDAR
scan at that moment, and colored lines depict the planned trajectory. (e1) and (e4) mark areas of LiDAR degeneration. (e2) and (e3) show obstacle avoidance.
(e5) and (e6) depict the camera first-person view from indoor to outdoor, highlighting large illumination variation from sudden overexposure to normal (see our
accompanying video on YouTube).2

is computationally expensive, necessitating extensive points for III. SYSTEM OVERVIEW

residual construction and rendering. In contrast, our sparse direct
The overview of our system is shown in Fig. 2, which contains
method provides enhanced computational efficiency. Finally,
four sections: ESIKF (see Section IV), local mapping (see
our system exploits information at the resolution of raw image Section V), LiDAR measurement model (see Section VI), and
patches, whereas R3LIVE is capped at the resolution of its point
visual measurement model (see Section VII).
map.
The asynchronously sampled LiDAR points are first recom-
The visual module of our system is most similar to DV- bined into scans at the camera’s sampling time through scan
LOAM [39], SDV-LOAM [40], and LVIO-Fusion [41], which
recombination. Then, we tightly couple the LiDAR, image and
projects LiDAR points attached with patches into a new image inertial measurements via an ESIKF with sequential state update,
and tracks the image by minimizing the direct photometric error. where the system state is updated sequentially, first by the
However, they have several key differences, such as the use of
LiDAR measurements and then by the image measurements,
separate maps for vision and LiDAR, reliance on the assumption both utilizing direct methods based on a single unified voxel map
of constant depth in patch warping in the visual module, loosely (see Section IV). To construct the LiDAR measurement model
coupling at the state estimation level, and two stages of frame-to-
in the ESIKF update (see Section VI), we compute the frame-
frame and frame-to-keyframes for image alignment. In contrast, to-map point-to-plane residual. To establish visual measurement
our system tightly integrates frame-to-map image alignment, model (see Section VII), we extract the visual map points within
LiDAR scan registration, and IMU measurements in an iterated
the current FoV from the map, making use of visible voxel query
Kalman filter. Besides, thanks to the single unified map for both and on-demand raycasting; after the extraction, we identify and
LiDAR and visual modules, our system can directly employ the discard outlier visual map points (e.g., points that are occluded
plane priors provided by the LiDAR points to accelerate the
or exhibit depth discontinuity); we then compute frame-to-map
image alignment. image photometric errors for visual update.
The local map for both visual and LiDAR updates is a voxel-
2 [Link]
map structure (see Section V): the LiDAR points construct and

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
330 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

Fig. 2. System overview of FAST-LIVO2.

TABLE I
SOME IMPORTANT NOTATIONS

Fig. 3. Illustration of scan recombination, forward propagation and backward

propagation applied to input data.
update the map’s geometric structure, while the visual images
append image patches to selected map points (i.e., visual map
points) and update reference patches dynamically. The updated ⎡ ⎤
reference patches have their normal vectors further refined in a ω m − bg − ng
⎢G vI + 1 (G RI (am − ba − na ) + G g)Δt⎥
separate thread. ⎢ 2 ⎥
⎢ G
RI (am − ba − na ) + G g ⎥
⎢ ⎥
⎢ ⎥
f (x, u, w) = ⎢ nbg ⎥
IV. ESIKF WITH SEQUENTIAL STATE UPDATE ⎢ ⎥
⎢ nba ⎥
This section outlines the system’s architecture, based on the ⎢ ⎥
⎣ 03×1 ⎦
sequentially updated ESIKF framework.
nτ
(2)
A. Notations and State Transition Model
In our system, we assume the time offsets among the three
where G RI , G pI , and G vI denote the IMU attitude, position,
sensors (LiDAR, IMU, and camera) are known, which can
and velocity in the global frame, respectively, G g is the gravity
be calibrated or synchronized in advance. We take IMU frame
vector in the global frame, τ is the inverse camera exposure time
(denoted as I) as the body frame and the first body frame as the
relative to the first frame, nτ is the Gaussian noise that models τ
global frame (denoted as G). Besides, we assume that the three
as a random walk, ω m and am are the raw IMU measurements,
sensors are rigidly attached and the extrinsic, defined in Table I,
ng and na are measurement noises in ω m and am , ba and bg
are precalibrated. Then, the discrete state transition model at the
are IMU bias, which are modeled as random walk driven by
ith IMU measurement is
Gaussian noise nbg and nba , respectively.
xi+1 = xi  (Δtf (xi , ui , wi )) (1)
where Δt is the IMU sample period, the state x, input u, process
noise w, and function f are defined as follows: B. Scan Recombination
We employ the scan recombination to segment the high-
M  SO(3) × R16 , dim(M) = 19 frequency, sequentially sampled LiDAR raw points into distinct
 T LiDAR scans at the camera sampling moments, as depicted in
x  G RTI G pTI G vIT bTg bTa G gT τ ∈ M
Fig. 3. This ensures both camera and LiDAR data are synchro-
 T  T nized at the same frequency (e.g., 10 Hz), allowing for update
u  ω Tm aTm , w  nTg nTa nTbg nTba nτ of state at the same time.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 331

Algorithm 1: Sequential State Update. yl and image measurements yc . However, LiDAR and image
measurements are two different sensing modalities, whose data
dimensions do not match. Furthermore, the fusion of image
measurement may be performed at various levels of the image
pyramid. To address the dimension mismatch and give more flex-
ibility for each module, we propose a sequential update strategy.
This strategy is theoretically equivalent to the standard update
using all measurements, assuming statistical independence of
LiDAR measurements yl and image measurements yc given
the state vector x (i.e., measurements corrupted by statistically
independent noise).
To introduce the sequential update, we rewrite the total con-
ditional distribution for the current state x as
p (x | yl , yc ) ∝ p (x, yl , yc ) = p (yc | x, yl ) p (x, yl )
= p (yc | x) p (yl | x) p (x) . (5)
  
∝p(x|yl )
Equation (5) implies that the total conditional distribution p(x |
yl , yc ) can be obtained by two sequential Bayesian updates. The
first step fuses only the LiDAR measurement yl with the IMU-
propagated prior distribution p(x) to obtain the distribution p(x |
yl )
p (x | yl ) ∝ p (yl | x) p (x) . (6)
The second step then fuses the camera measurement yc with
p(x | yl ) to obtain the final posterior distribution of x
p (x | yl , yc ) ∝ p (yc | x) p (x | yl ) . (7)
Interestingly, the two fusion in (6) and (7) follow the same form
q (x | y) ∝ q (y | x) q (x) . (8)
To conduct the fusion in (8) for either LiDAR or image
measurements, we detail the prior distribution q(x) and mea-
surement model q(y | x) as follows. For the prior distribution
C. Propagation q(x), denote it as x = x  δx with δx ∼ N (0, P). In case of
the LiDAR update (i.e., the first step), (x, P) is the state and
In the ESIKF framework, the state and covariance are prop-
covariance obtained from the propagation step. In case of the
agated from time tk−1 , when the last LiDAR scan and image
frame are received, to time tk , when the current LiDAR scan visual update (i.e., the second step), (x, P) is the converged
and image frame are received. This forward propagation predicts state and covariance obtained from the LiDAR update.
the state at each IMU input ui during tk−1 and tk , by setting the To obtain the measurement model distribution q(y | x), de-
process noise wi in (1) to zero. Denote the propagated state as x note state estimated at the κth iteration as xκ , where x0 = x.
Approximating the measurement model (4) (either the LiDAR
and covariance as P, which will serve as a prior distribution for
or camera measurement) through its first-order Taylor expansion
the subsequent update in Section IV-D. Moreover, to compensate
made at xκ leads to
for motion distortion, we conduct a backward propagation as
in [42], ensuring points in a LiDAR scan are “measured” at the y | x  h (xκ , 0) +Hκ δxκ + Lκ v (9)
  
scan-end time tk . Note that for notation simplification, we omit zκ
the subscript k in all state vectors.
q(y | x)  N (h (xκ , 0) + Hκ δxκ , R) (10)
D. Sequential Update where δx = x  xκ , zκ is the residual, Lκ v ∼ N (0, R) is
κ

the lumped measurement noise, Hκ and Lκ are the Jacobian

The IMU propagated state x and covariance P impose a prior matrices of h(xκ  δxκ , v) with respect to δxκ and v, evaluated
distribution for x, the system state at time tk , as follows: at zero, respectively.
 Then, substituting the prior distribution q(x) and the measure-
x  x ∼ N 0, P . (3) ment distribution q(y | x) in (10) into the posterior distribution
We denote the above-mentioned prior distribution as p(x) and (8) and performing maximum likelihood estimation, we can
the measurement models for the LiDAR and camera as obtain the maximum a-posterior estimation of δxκ (and hence
    xκ ) from the standard update step in the ESIKF framework [43]
yl hl (x, vl ) −1
= (4)
yc hc (x, vc ) K = (Hκ )T R−1 Hκ + P−1 (Hκ )T R−1
where vl ∼ N (0, Σvl ) and vc ∼ N (0, Σvc ) denote the mea-
surement noises for the LiDAR and camera, respectively. xκ+1 = xκ (−Kzκ − (I − KHκ ) (xκ  x)) . (11)
A standard ESIKF [43] would update the state x using all The converged state and covariance matrix then make the mean
the current measurements, including both LiDAR measurement and covariance of the posterior distribution q(x|y).
Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
332 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

distance d. As the map moves, the memory containing the area

moved out of the local map will be reset to store new areas
moved in the local map. This ring-buffer approach ensures that
our local map is maintained within a fixed size of memory. The
implementation of the ring-buffer Hash map is detailed in [46].
The map move check is performed after each ESIKF update step.

B. Geometry Construction and Update

Fig. 4. 2-D demonstration of local map slide. In (a), the gray rectangle is The geometry of the map is constructed and updated from
the initial map region with length L. The red circle is the initial detection area LiDAR point measurements. Specifically, after the LiDAR up-
centered at p0 . In (b), the detection area moves to a new position p1 where date in ESIKF (see Section IV), we register all points from the
the map boundaries are touched. The map region is moved to a new position
(blue rectangle) by distance d. In (c), the memory space B remains unchanged.
LiDAR scan to the global frame. For each registered LiDAR
The memory space A storing the green area is reset for the blue area C in (b). point, we determine its located root voxel in the Hash map. If
(a) Initial map. (b) Move the map. (c) Reset the memory. the voxel does not exist, we initialize the voxel with the new point
and index it into the Hash map. If the determined voxel already
exists in the map, we append the point to the existing voxel.
Kalman filter with sequential update has been investigated After all points in a scan are distributed, we conduct geometry
in the literature, such as in [44] and [45]. This article adopts construction and update as follows.
this approach for ESIKF for LiDAR and camera systems. The For newly created voxels, we determine if all the contained
implementation of the ESIKF with sequential update is detailed points lie on a plane based on the singular value decomposi-
in Algorithm 1. In the first step (Lines 6–10), the error state tion. If so, we calculate the center point q = p̄, plane normal
is updated from the LiDAR measurements (see Section VI-A) n, and the covariance matrix of (q, n), denoted as Σn,q , of
iteratively until convergence. The converged state and covari- the plane. Σn,q is used to characterize the plane uncertainty,
ance estimates, denoted again as x and P, are used to update the which arises from both pose estimation uncertainty and the point
geometry of the map (see Section V-B), and subsequently refined measurement noise. The detailed plane criteria and calculation
in the second step visual update (Lines 13–23) on each level of the plane parameters and uncertainties can be referred to our
of the image pyramid (see Section VII-B) until convergence. previous work [14]. If the contained points do not lie on a plane,
The optimal state and covariance, denoted as x̄ and P̄, are the voxel is continuously subdivided into eight smaller octants
employed for propagating incoming IMU measurements (see until either the points in the subvoxel are determined to form
Section IV-C) and update the visual structures of the map (see a plane or the maximum layer (e.g., 3) is reached. In the latter
Sections V-D and V-E). case, the points in the leaf voxel will be discarded. As a result,
the map only contains voxels (either root or sub) identified as
V. LOCAL MAPPING planes.
For existing voxels that have new points appended, we assess
A. Map Structure if the new points still form a plane with the existing points in the
Our map employs an adaptive voxel structure presented root voxel or subvoxel. If not, we conduct voxel subdivision
in [14], which is organized by a Hash table and an octree for each as previous. If yes, we update the plane’s parameters (q, n)
Hash entry (see Fig. 2). The hash table manages root voxels, and covariance Σn,q as above-mentioned too. Once the plane
each with a fixed dimension of 0.5 × 0.5 × 0.5 m. Each root parameters converge (see [14]), the plane will be considered as
voxel encapsulates an octree structure to further organize leaf mature and new points on this plane will be discarded. Moreover,
voxels of varying sizes. A leaf voxel represents a local plane mature planes will have their estimated plane parameters (q, n)
and stores a plane feature (i.e., plane center, normal vector, and and covariance Σn,q fixed.
uncertainty) along with a set of LiDAR raw points situated on LiDAR points on planes (either in the root voxel or subvoxel)
this plane. Some of these points are attached with three-level will be used for generating visual map points in the subsequent
image patches (8 × 8 patch size), which we refer to as visual section. For mature planes, 50 most recent LiDAR points are
map points. Converged visual map points are only attached with candidates for visual map point generation, while for immature
reference patches, while nonconverged ones are attached with planes; all LiDAR points are the candidates. The visual map
reference patches and other visible patches (see Section V-E). point generation process will identify some of these candidate
The varying size of the leaf voxel allows it to represent local points as visual map points and attach them with image patches
planes of different scales, thus being adaptable to environments for image alignment.
with different structure [14].
To prevent the size of the map from going unbound, we keep
only a local map within a large local region of length L around C. Visual Map Point Generation and Update
the LiDAR’s current position, as illustrated in a 2-D example To generate and update visual map points, we select the
in Fig. 4. Initially, the map is a cube centered at the LiDAR’s candidate LiDAR points in the map that 1) are visible from
starting position p0 . The LiDAR’s detection area is visualized as the current frame (detailed in Section VII-A), and 2) exhibit
a sphere centered at its current position, with its radius defined significant gray-level gradients in the current image. We project
by the LiDAR’s detection range. When the LiDAR moves to a these candidate points, after the visual update (see Section IV-D),
new position p1 where the detection area touches the boundaries onto the current image and retain the candidate point with the
of the map, we move the map away from the boundaries by a smallest depth for the local plane in each voxel. Then, we divide

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 333

the current image into uniform grid cells each with 30 × 30 pix-
els. If a grid cell does not contain any visual map point projected
here, we generate a new visual map point using the candidate
point with the highest gray-level gradient and associate it with
the current image patch, estimated current state (i.e., frame pose
and exposure time), and the plane normal calculated from the
LiDAR points as in the previous section. The patch attached to
the visual map points has three layers of the same size (e.g.,
11 × 11 pixels), each layer is half sampled from the previous
layer, forming a patch pyramid. If a grid cell contains visual
map points projected here, we add new patch (all three layers
of pyramid) to the existing visual map point if 1) more than 20
frames have passed since its last patch addition, or 2) its pixel Fig. 5. (a) Affine warping between reference patches and target patches.
position in the current frame deviates by more than 40 pixels (b) Any normal Ir n ∈ S2 lying on the normalized sphere is first projected
T
from its position at the last patch addition. As a result, the into a point M ∈ R3 on the plane Ir p M = 1, and then projected to a point
2
m ∈ R on the x-y plane. This transformation thereby converts a perturbation
map points will likely have effective patches with uniformly δn on the sphere into a perturbation δm in x-y plane.
distributed viewing angles. Along with the patch pyramid, we
also attach the estimated current state (i.e., pose and exposure
time) to the map point. E. Normal Refine
Each visual map point is assumed to lie on a small local plane.
D. Reference Patch Update Existing works [2], [4], [8] assumed that all pixels in a patch
A visual map point could have more than one patch due to have the same depth, a wild assumption that does not hold in
the addition of new patches. We need to choose one reference general. We use plane parameters computed from the LiDAR
patch for image alignment in the visual update. In detail, we points as detailed in Section V-B to achieve greater accuracy.
score each patch f based on photometric similarity and viewing This plane normal is crucial for performing affine warping for
angle as follows: image alignment in the visual update process. To further enhance
   the accuracy of the affine warping, the plane normal could be
x,y f (x, y) − f̄ [g(x, y) − ḡ] further refined from the patches attached to the visual map point.
NCC (f , g) =   2  Specifically, we refine the plane normal in the reference patch
2
x,y f (x, y) − f̄ x,y [g(x, y) − ḡ] by minimizing the photometric error with respect to the other
n·p 1 patches attached to the visual map point.
c= , ω1 = 1) Affine Warping: Affine warping is used to transform patch
p 1 + etr(Σn ) pixels from the reference frame (i.e., the source patch) to patch
n
1 pixels in the rest of the frames (i.e., the target patch), illustrated
S = (1 − ω1 ) · NCC (f , gi ) + ω1 · c (12) in Fig. 5(a). Let ujr be the jth pixel coordinates in the source
n i=1
patch and uji be the jth pixel coordinates in the ith target patch.
where NCC(f , g) represents the normalized cross-correlation Assuming all pixels in the patch lie in a local plane with normal
Ir
(NCC) used to measure the similarity between patch f and g n and visual map point position Ir p (which corresponds to the
at the 0th pyramid level (the level with the highest resolution) center pixel for both source and target patches), both represented
of both patch, with mean subtraction applied to both patches, in the source patch frame, we have
c denotes the cosine similarity between the normal vector n
uji = Air ujr
and view direction p/p of patch f under evaluation. When
 
the patch is directly facing the plane where the map point is 1
located, the value of c is 1. The overall score S is calculated by Ar = P Ii RIr + Ii tIr I
i Ir T
n P−1 (13)
r nT · Ir p
summing the weighted NCC and c, where the former represents
the average similarity between the patch f under evaluation where Air represents the affine warping matrix that transforms
and all other patches gi and tr(Σn ) represents the trace of the the pixel coordinates from the source (or reference) patch to
covariance matrix of the normal vector. the ith target patch, Ii RIr and Ii tIr denote the relative pose
Among all the patches attached to a visual map point, the one of the reference frame Ir w.r.t. the target frame Ii . To use fisheye
with the highest score is updated as the reference patch. The images directly without rectifying them to pinhole images, we
above-mentioned scoring mechanism tends to choose reference implement projection matrix P and back projection matrix P−1
patches whose 1) appearance is similar (in terms of NCC) to based on different camera models (e.g., P is the camera intrinsic
most of the rest of the patches, a technique used by MVS [47] to matrix for the pinhole camera model).
avoid patches on dynamic objects; 2) view direction is orthog- 2) Normal Optimization: To refine the plane normal Ir n, we
onal to the plane, thereby maintaining texture details at a high minimize photometric errors between the reference patch and
resolution. In contrast, the reference patches update strategy in other image patches at the 0th pyramid level (i.e., the highest
our previous work FAST-LIVO [8] and prior arts [4] directly resolution level)
select the patch with the smallest view direction difference from 2
N

the current frame, causing the selected reference patch to be very     
Ir ∗
n = arg min τi Ii Air ujr − τr Ir ujr  (14)
close to the current frame, hence imposing weak constraints on Ir n∈S2 2
the current pose update. i∈S j=1

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
334 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

where N is the path size, τr and τi are the inverse exposure times
of the reference frame and the ith target frame, respectively.
Ir (ujr ) denote jth patch pixel in the reference frame, Ii (Air ujr )
denotes the jth path pixel in the ith target frame, and S is the
set of all target frames.
3) Optimization Variable Transformation: To enhance the
computational efficiency, we reparameterize the least squares
problem in (14). Note that the optimization variable Ir n only
appeared in M  Ir nT1·Ir p Ir n ∈ R3 in (13), the optimization
over Ir n can be conducted over M. Moreover, the vector M
is subject to constraint Ir p · M = 1, meaning that M can be
parameterized as follows:
⎡ ⎤
Mx
⎢ My ⎥
M=⎣ ⎦ = Bm + b
Ir
1 Ir
px py
Ir p
z
− I r p Mx − I r p My
z z
⎡ ⎤ ⎡ ⎤ Fig. 6. (a) and (b), respectively, illustrate the 3-D and side crosssectional views
1 0 0   of the LiDAR point uncertainty model considering the laser beam divergence
⎢ 0 ⎥ ⎢
1 ⎦,b = ⎣ 0 ⎦,m = ⎥ M x angle θ. The red contour outlines the area that a laser beam spreads. (c) and
B=⎣ ∈ R2
Ir
px
Ir
py 1 My (d) color the points in a scan by the point location uncertainty. Compared to
− Ir p − Ir p Ir p
z
(c), (d) further takes into account the ranging uncertainty δd resulting from the
z z
beam divergence angle. This leads to a higher uncertainty for ground points due
(15) to the large spread area of laser beams.
where Ir pz = 0 since no such reference patch could be chosen
for the visual map point. The relation among Ir n, M, and m are
shown in Fig. 5(b). (nj , qj ) with covariance Σn,q (see Section V-B), so we have:
Finally, the optimization in (14) is conducted over the vector ngt gt
j = nj  δnj , qj = qj − δqj . Therefore
m ∈ R2 without any constraints. This optimization can be per- G L  
formed in a separate thread to avoid blocking the main odometry 0 = (nj  δnj )T
 TI I TL pj −δ L pj −(qj −δqj )
thread. The optimized parameter m∗ can then be used to recover   
yl
hl (x,vl )
the optimal normal vector Ir n∗
(19)
Ir ∗ M∗
n = , M∗ = Bm∗ + b. (16)
M∗  where the measurement noise vl = (δ L pj , δnj , δqj ) consist-
Once the plane normal converges, the reference patch and nor- ing of the noise associated with the LiDAR point, the normal
mal vector for this visual map point are fixed without further vector, and the plane center, respectively.
refinement, and all other patches are deleted.
B. LiDAR Measurement Noise With Beam Divergence
VI. LIDAR MEASUREMENT MODEL
The uncertainty of a LiDAR point δ L pj in the local LiDAR
This section details the LiDAR measurement model yl = frame is decomposed into two components in [14], the ranging
hl (x, vl ) used in the LiDAR update of ESIKF in Section IV-D. uncertainty δd caused by laser time of flight (TOF), and the
bearing direction uncertainty δω originated from encoders. Be-
A. Point-to-Plane LiDAR Measurement Model sides these uncertainties, we also consider uncertainties caused
After obtaining the undistorted points {L pj } in a scan, we by the laser beam divergence angle θ, as illustrated in Fig. 6.
project them to the global frame using the estimated state xκ at As the angle ϕ between the bearing direction and normal vector
the κth iteration of LiDAR update increases, the ranging uncertainty of the LiDAR point increases
G κ
significantly, while the bearing direction uncertainty remains
pj = G TκI I TL L pj . (17) unaffected. The δd due to the laser beam divergence angle can
G κ be modeled as
We then identify the root or subvoxel where pj
lies in the
Hash map. If no voxel is found or the voxel does not contain a  
cos ϕ cos ϕ
plane, the point is discarded. Otherwise, we use the plane in the δd = L2 − L1 = d − . (20)
voxel to establish a measurement equation for the LiDAR point. cos(θ + ϕ) cos(θ − ϕ)
Specifically, we assume the true LiDAR point L pgt j , given the Considering δd influenced by TOF and laser beam divergence,
G
accurate LiDAR pose TI , should lie on the plane with normal when our system selects more points from the ground or walls
ngt gt
j and center point qj in the voxel. i.e.,
[see Fig. 6(c) and (d)], it achieves a more precise pose estimation
 T G I gt 
than that not considering such effect.
0 = ngt j TI TL L pgt
j − qj . (18)
Since the ground-true point L pgt j is measured as
L
pj with VII. VISUAL MEASUREMENT MODEL
gt
ranging and bearing noises δ pj , we have pj = pj − δ L pj .
L L L
This section details the visual measurement model yc =
Likewise, the plane parameters (ngt gt
j , qj ) are estimated as hc (x, vc ) used in the visual update of ESIKF in Section IV-D.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 335

Fig. 8. Outlier rejection. (a) shows the diagrammatic drawing of occluded and
depth-discontinuous visual map points. (b) shows the effect of outlier rejection
in real scenes. The red dots are the rejected visual map points, and the green
dots are the accepted visual map points.

Fig. 7. Illustration of on-demand voxel raycasting.

through raycasting, we obtain a set of visual map points that
distribute among the whole image.
3) Outlier Rejection: After voxel query and raycasting, we
A. Visual Map Point Selection obtain all visual map points in the current frame FoV. However,
these visual map points could be occluded in the current frame,
To perform sparse image alignment in the visual update, we have discontinuous depth, have their reference patch taken at
begin by selecting appropriate visual map points. We first extract large view angles, or have large view angles in the current frame,
the set of map points (termed as visual submap) that is visible all of which can severely degrade image alignment accuracy.
in the current camera FoV, using voxel, and raycasting queries. To address the first issue, we project all the visual map points
Then, the visual map points from this submap are selected and in the submap into the current frame using the pose after the
outliers are rejected. This process yields a refined set of visual LiDAR update and keep the lowest depth points in each grid
map points ready for constructing visual photometric errors in cell of 30 × 30 pixels. To address the second issue, we project
the visual measurement model. the LiDAR points in the current LiDAR scan to the current
1) Visible Voxel Query: Identifying map voxels within the frame producing a depth map. By comparing the depth of visual
current frame FoV is challenging due to the large number of map points with their 9 × 9 neighbor in the depth map, we
voxels in the map. To address this issue, we poll the voxels hit by determine their occlusion and depth variation. Occluded and
LiDAR points in the current scan. This can be done efficiently by depth-discontinuous map points are rejected (see Fig. 8). To
inquiring the voxel Hash table using the measured point position. address the third and fourth issue, we remove points where the
If the camera FoV is largely overlapped with the LiDAR FoV, view angle (i.e., the angle between normal vector and direction
map points in the camera FoV likely lie in these voxels as well. from the visual map point to the patch optical center) of the
We also poll voxels hit by map points identified visible (through reference patch or current patch is too large (e.g., over 80◦ ). The
the same voxel query and raycasting) in the previous image remaining visual map points will be used to align the current
frame, assuming that two consecutive image frames have large image.
FoV overlaps. Finally, the current visual submap can be obtained
as map points contained in these two types of voxels followed
by an FoV check. B. Sparse-Direct Visual Measurement Model
2) Raycasting on Demand: In most cases, the visual submap The visual map points {G pi } extracted previously are used
can be obtained through the voxel queries above. However, a to construct the visual measurement model. The underlying
LiDAR sensor could return no points when it is too close to principle is that, when transforming the map point G pi to the
an object (known as the close proximity blind zones). Also, the current image Ik (·) with the ground-truth state (i.e., pose) xk , the
camera FoV may not be completely covered by the LiDAR FoV. photometric error between the reference patch and the current
To recall more visual map points in these cases, we employ patch should be zero
a raycasting strategy, as illustrated in Fig. 7. We divide the C 
image into uniform grid cells, each with 30 × 30 pixels, and 0 = τk Igt
k (π TI (G TI )−1G pi +Δu)
  
project the visual map points obtained from the voxel query ui
onto the grid cells. For each image grid cell that is not occupied
by these visual map points, a ray is cast backward along the − τr Igt Cr G r
r (π( TG pi ) +Ai Δu) (21)
  
central pixel, where sample points are uniformly distributed ui
along the ray in the depth direction from dmin to dmax . In order
to reduce computation load, the positions of sample points on where π(·) is the common camera projection model (i.e., Pin-
each ray in the camera body frame are precomputed. For each hole, MEI, ATAN, Scaramuzza, Equidistant), Cr TG is the pose
sampled point, we evaluate the corresponding voxel’s status: of the global frame G w.r.t. reference frame Cr , which has been
if the voxel contains map points that lie in this grid cell after estimated when receiving and fusing the reference frame, Ari
projection, we incorporate these map points into the visual is the affine warping matrix that transforms pixels from the ith
submap and cease for this ray. Otherwise, we continue to the current patch to the reference patch, Δu is the relative pixel
next sample point on the ray until reaching the maximum depth position to the center ui within the current patch, Igt gt
k , Ir denote
dmax . After processing the entire unoccupied image grid cells the ground-true pixel values of the reference and current frames,

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
336 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

respectively. They are measured as the actual image pixel values OS1 gen13 LiDAR sampled at 10 Hz and with a built-in IMU
Ik , Ir with measurement noise vc = (δIk , δIr ), which originate at 100 Hz, and two synchronized pinhole cameras triggered at
from various sources [e.g., shot noise and the analog-to-digital 10 Hz. The left camera is used for evaluation.
converter (ADC) noise of the camera CMOS]. Hence The Hilti’22 and Hilti’23 datasets, collected by handheld and
0 = τk (Ik (ui +Δu)−δIk )−τr (Ir (ui +Ari Δu)−δIr ) .

robot devices, encompass indoor and outdoor sequences from
   environments, such as construction sites, offices, labs, and park-
yc hc (x,vc ) ing areas. These sequences introduce numerous challenges from
(22) long corridors, basements, and stairs, with textureless features,
To enhance computational efficiency, we employ an inverse varying illumination conditions, and insufficient LiDAR plane
compositional formulation [4], [48], where the pose incremental constraints. Handheld sequences use a Hesai PandarXT-324
δT ∈ R6 parameterizing G TI =GTκI Exp(δT) in ui [see (21)], LiDAR at 10 Hz, five wide-angle cameras at 40 Hz, which is
is moved from ui to ui as follows: downsampled into 10 Hz, and an external Bosch BMI085 IMU
 −1  at 400 Hz. Meanwhile, the robot-mounted sequences feature
ui = π C TI G TκI G
pi a Robosense BPearl5 LiDAR at 10 Hz, eight omnidirectional
cameras at 10 Hz, and an Xsens MTi-670 IMU at 200 Hz. In
 both cases, the front-facing camera is used for all systems under
ui = π Cr
TG Exp (δT)G pi . (23) evaluation. Millimeter-accurate ground truth, obtained through
a motion capture system or a Total Station [54], is provided for
Given that ui in the reference frame remains unchanged during each sequence. Note that the ground truth of the Hilti datasets is
each iteration, we only require a one-time computation of the not open-source; therefore, algorithmic results on these datasets
Jacobian matrices w.r.t. δT, rather than recalculating them for are evaluated via the Hilti official website. Since “Site 3” in
every iteration. Hilti’23 does not provide in-depth analysis plots (e.g., RMSE),
To estimate the inverse exposure time τk from the measure- we exclude these four sequences, but our scoring results for
ment (22), we fix the initial inverse exposure time τ0 = 1 to these sequences can still be found on their official website.6
eliminate the degeneration of (22) when all inverse exposure NTU-VIRAL and Hilti contribute a total of 25 sequences.
time are zeros. The estimated inverse exposure times of subse- The MARS-LVIG dataset provides high-altitude, ground-
quent frames are therefore the exposure time relative to the first facing mapping data that encompasses diverse unstructured
frame. terrains, such as jungles, mountains, and islands. The dataset
Equation (22) is used in the visual update step across three was collected via a DJI M300 RTK quadrotor, which is equipped
levels (see Algorithm 1); the visual update starts from the with a Livox Avia7 LiDAR (with built-in BMI088 IMU) and a
coarsest level, after the convergence of a level, it proceeds to high-resolution global-shutter camera, both triggered at 10 Hz.
the next finer level. The estimated state is then used to generate This is notably distinct from the aforementioned NTU-VIRAL
visual map points (see Section V-C) and update reference patch and Hilti datasets, which use 752 × 480 grayscale images, while
(see Section V-D). the MARS dataset employs 2448 × 2048 RGB images, thereby
facilitating the generation of clear, dense colored point clouds.
VIII. DATASETS FOR EVALUATION Therefore, we leverage this public dataset to validate our capa-
In this section, we introduce datasets for performance evalua- bilities in high-altitude aerial mapping applications.
tion, including public datasets NTU-VIRAL [49], Hilti’22 [50],
Hilti’23 [51], and MARS-LVIG [52], as well as our self- B. FAST-LIVO2 Private Dataset
collected FAST-LIVO2 private dataset. Specifically, the NTU- To validate the system’s performance under more extreme
VIRAL and Hilti datasets are used to conduct a quantitative conditions (e.g., LiDAR degeneration, low illumination, drastic
benchmark comparison of our system against other SOTA exposure changes, and cases of no LiDAR measurements), we
SLAM systems (see Section IX-B). The FAST-LIVO2 pri- make a new dataset named FAST-LIVO2 private dataset. The
vate dataset is primarily used to evaluate our system across dataset, hardware device, and hardware synchronization scheme
various extremely challenging scenarios (see Section IX-C), are released with the codes of this work to facilitate the repro-
to demonstrate its capability for high-precision mapping (see duction of our work.
Section IX-D), and to validate the functionality of the individual 1) Platform: Our data collection platform, illustrated in
modules within our system (see Sections I-A through I-D in Fig. 9, is equipped with an industrial camera (MV-CA013-
the Supplementary Material [53]). MARS-LVIG dataset is em- 21UC), a Livox Avia LiDAR, and a DJI manifold-2c (Intel
ployed for application demonstrations (see Section X) and abla- i7-8550u CPU and 8GB RAM) as onboard computers. The cam-
tion study (see Section I-E in the Supplementary Material [53]). era FoV is 70.6◦ × 68.5◦ and the LiDAR FoV is 70.4◦ × 77.2◦
. All sensors are hard synchronized with a 10 Hz trigger signal,
A. NTU-VIRAL, Hilti, and MARS-LVIG Dataset generated by STM32 synchronized timers.
The NTU-VIRAL dataset, collected at the Nanyang Tech- 2) Sequence Description: As summarized in Table S1 in
nological University campus using an aerial platform, presents the Supplementary Material [53], the FAST-LIVO2 dataset
diverse scenarios embodying unique aerial operational chal- 3 [Online].
lenges. Specifically, the “sbs” sequences can only provide noisy Available: [Link]
4 [Online]. Available: [Link]
visual features from distant objects. The “nya” sequences present 5 [Online]. Available: [Link]
challenges to LiDAR SLAM due to semitransparent surfaces 6 [Online]. Available: [Link]
and to visual SLAM owing to intricate flight dynamics and low [Link]
lighting conditions. The dataset is equipped with a 16-channel 7 [Online]. Available: [Link]

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 337

implementation of FAST-LIVO2 on the ARM-based platform

as “FAST-LIVO2 (ARM).”

B. Benchmark Experiments
In this experiment, we conduct quantitative evaluations on
25 sequences from the NTU-VIRAL, Hilti’22, and 23 open
datasets. Our approach is benchmarked against several SOTA
open-source odometry systems, including R3LIVE [36], a dense
direct LIVO system; FAST-LIO2 [13], a direct LiDAR–inertial
odometry system; SDV-LOAM [40], a semidirect LiDAR-visual
odometry system; LVI-SAM [35], a feature-based LiDAR–
Fig. 9. Our platform with hardware synchronization for data acquisition. inertial–visual SLAM system; and our previous work FAST-
(a) Our handheld platform. (b) Hardware synchronization scheme.
LIVO [8].
These systems are downloaded from their respective GitHub
comprises 20 sequences across various scenes (e.g., campus repositories. For FAST-LIO2, FAST-LIVO, and LVI-SAM, we
buildings, corridors, basements, mining tunnel, etc.) charac- use the recommended settings for indoor and outdoor scenes
terized by structure-less, cluttered, dim, variable-lighting, and equipped with multiline LiDAR sensors. For R3LIVE, we adapt
weakly textured environments, with a total duration of 66.9 min. the system to work with fisheye camera models and multiline
Most sequences exhibit visual and/or LiDAR degeneration, such LiDARs equipped with external IMUs (the default configura-
as facing a single and/or texture-less plane, traversing an ex- tion only supports internal IMUs). We disable the real-time
tremely narrow and/or dark tunnel, and experiencing varying optimization of the camera intrinsic and the extrinsic C TI due
light conditions from indoor to outdoor (see Fig. S7 in the Sup- to adverse optimization caused by insufficient IMU excitation
plementary Material [53]). To guarantee enhanced synchronous in the datasets. Other parameters, including the window size
data collection between the camera and LiDAR, we configure and pyramid level for optical flow tracking, the resolution for
the camera with fixed exposure time but autogain mode in most downsampling the point cloud of the current scan and the global
scenarios. For the remaining sequences with autoexposure, we map, are fine-tuned to achieve optimal performance. Since only
record their ground truth exposure times. In all sequences, the the vision module of SDV-LOAM is open-sourced, we integrate
platform returns to the starting point, which enables the drift it with LeGO-LOAM [7] in a loosely coupled manner, following
evaluation. the methodology described in the original paper [40]. This en-
hanced system continues to refine poses obtained from the vision
IX. EXPERIMENT RESULTS module and we also open this implementation on GitHub.9 Given
that all compared systems are odometry without loop closure,
In this section, we conduct extensive experiments to evaluate except for LVI-SAM, we remove the loop-closure module of
our proposed system. LVI-SAM to ensure a fair comparison. In addition, we conduct
an ablation study on the exposure time estimation module, the
A. Implementation and System Configurations normal refine module, and the reference patch update strategy.
We implemented the proposed FAST-LIVO2 system in C++ The default FAST-LIVO2 has real-time exposure estimation and
and robots operating system. In the default configuration, the reference patch update, but no normal refinement.
exposure time estimation is enabled, while normal vector refine- The results of all methods are shown in Table II. It is seen
ment is turned OFF. LiDAR points in a scan are downsampled that our method achieves the highest overall accuracy across
temporally at a 1:3 ratio. The root voxel size for the voxel map all sequences with an average RMSE of 0.044 m, which is
is set at 0.5 m, and the maximum layer of the internal octree is 3. three times more accurate than the second-place FAST-LIVO at
The image patch size is 8 × 8 for image alignment and 11 × 11 0.137 m. Our system delivers the best results in most sequences,
for normal refinement. Within the sequential ESIKF settings, except for “Outside Building” and “Large Room (dark),” where
for all the experiments, the camera photometric noise is set to a our system exhibits a slightly (millimeter-level) higher error
constant value of 100. The LiDAR depth error and bearing angle compared to the LiDAR–inertial only odometry FAST-LIO2.
error are adjusted to 0.02 m and 0.05◦ for Livox Avia LiDAR This discrepancy can be attributed to the rich structural features
and OS1-16, 0.001 m and 0.001◦ for PandarXT-32, 0.008 m and but poor lighting conditions of these sequences, resulting in
0.01◦ for Robosense BPearl LiDAR. The laser beam divergence dim and blurred images. Consequently, fusing these low-quality
angle is set at 0.15◦ for Livox Avia LiDAR and OS1-16, and at images does not enhance odometry accuracy. Excluding
0.001◦ for the PandarXT-32 and Robosense BPearl LiDAR. Our these two sequences, our method, which leverages tightly
system uses the same parameters in all sequences of all datasets coupled LiDAR, inertial, and visual information, outperforms
with the same sensor setup. The computation platform for all FAST-LIO2, our LIO subsystem, and the LiDAR-visual only
experiments is a desktop PC equipped with an Intel i7-10700K odometry, SDV-LOAM, significantly. Notably, SDV-LOAM
CPU and 32GB RAM. For FAST-LIVO2, we also test it on an performs particularly poorly on the Hilti datasets due to its lack
ARM processor that is commonly used in embedded systems of tight integration with IMU measurements, leading to drift
with reduced power and cost. The ARM platform is RB5.8 with in the LO subsystem. In addition, the loose coupling between
a Qualcomm Kryo585 CPU and 8GB RAM. We refer to the LiDAR and visual observations, along with poor initial values for

8 [Online]. Available: [Link] 9 [Online]. Available: [Link]

Modules/SMARC/LEC-RB5?lang=en reimplementation

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
338 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

TABLE II
ABSOLUTE TRANSLATIONAL ERRORS (RMSE, METERS) IN SEQUENCES

VO, often results in local optima or even negative optimization. exposure time estimation can actively compensate illumination
Our LIO subsystem generally surpasses FAST-LIO2 due to changes in the environment. On the other hand, the average
our more accurate noise modeling for each LiDAR point. In accuracy without the reference patch update decreases by 44 mm
a few sequences where FAST-LIO2 outperforms slightly, the compared to the default, as the reference patch update strategy
differences are minimal, at the millimeter level, and negligible. effectively selects patches with higher resolution and avoids se-
Moreover, our system’s accuracy significantly exceeds that of lecting outlier patches. Finally, the normal refinement increases
other tightly coupled LiDAR–inertial–visual systems across the average accuracy by 1 mm, and the accuracy improvement
all sequences. Among them, LVI-SAM fails in nine sequences is not consistent in all sequences. The limited improvement is
primarily due to its feature-based LIO and VIO subsystems mainly because normal vector refinement yields positive op-
not fully utilizing raw measurements, which degrades its timization only in simple structured scenes with nice image
robustness in environments with subtle geometric or texture observations. In the NTU-VIRAL dataset, images from the
features. R3LIVE generally performs well, but struggles “eee” and “nya” sequences are extremely dim and blurry, where
in “Construction Stairs,” “Cupola,” and “Attic to Upper negative optimization is particularly severe. To further study
Gallery” sequences, where its performance is even worse than the effectiveness of the different modules, including exposure
FAST-LIO2. This is because intense rotations at structure-less time estimation, affine warping, reference patch update, nor-
staircases result in inadequate pose priors, causing local optima mal convergence, on-demand raycasting, and ESIKF sequential
when aligning colored map points with the current frame, and update, we conducted a thorough study on our private dataset
ultimately leading to negative optimization. FAST-LIVO and and MARS-LVIG dataset. The results are presented in Section I
FAST-LIVO2 overcome such challenges in these sequences by (system module validation) in the Supplementary Material [53]
the patch-based image alignments. In addition, situations where due to the space limit. As confirmed in the results, our system can
the sensors are close to walls in these sequences highlight achieve robust and accurate pose estimation in both structured
the effectiveness of raycasting in FAST-LIVO2, with the and unstructured environments, under severe light variations,
mapping results in these large-scale scenes shown in Fig. in remarkably large-scale scenarios with long-term, high-speed
S8 in the Supplementary Material [53]. On the other hand, data collection, and even in extremely narrow spaces with few
FAST-LIVO is outperformed by R3LIVE and FAST-LIVO2 LiDAR measurements.
on the NTU-VIRAL dataset, especially in unstructured scenes,
such as “nya” sequences, where the effects of affine warping C. LiDAR Degenerated and Visually Challenging
based on constant depth assumptions are inaccurate. In contrast,
Environments
the pixel-level alignment of R3LIVE and the plane prior (or
refinement) of FAST-LIVO2 do not encounter such issues. In this experiment, we evaluate the robustness of our system
Comparing the different variants of FAST-LIVO2, we observe under environments experiencing LiDAR degeneration and/or
that the average accuracy without real-time exposure time es- visual challenges, comparing it with the qualitative mapping
timation decreases by 6 mm compared to the default, as the results of FAST-LIVO and R3LIVE in eight sequences, as shown
Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 339

Fig. 10. Mapping results generated in real time in LiDAR degenerated scenes. The point clouds from top to bottom correspond to “Bright Screen Wall,” “Black
Screen Wall,” “HIT Graffiti Wall” (the third and fourth rows), “Banner Wall,” “HKU Lecture Center,” respectively, showing the comparison of colored point cloud
constructed FAST-LIVO2, FAST-LIVO, or R3LIVE (see more details on YouTube).10

in Figs. 10 and 11. Fig. 10 showcases LiDAR degeneration illustrating the areas of LiDAR degeneration due to the single
sequences where the LiDAR is facing a big wall while moving plane being observed. Furthermore, the “Mining Tunnel” ex-
along the wall from one side to the other. Due to the absence hibits very dim lighting throughout the whole sequence, coupled
of geometrical constraints since only one wall plane is being with frequent visual and LiDAR degeneration. Despite of these
observed by the LiDAR, LIO methods would fail. It is worth challenges, FAST-LIVO2 still returns to the starting point with
mentioning that the “HIT Graffiti Wall” sequence spans nearly an end-to-end error of less than 0.01 m in both sequences.
800 m with LiDAR continuously facing the wall, leading to con-
siderable degeneration. In all sequences, FAST-LIVO2 distinctly D. High-Precision Mapping
showcases its robustness against even long-term degeneration
and its capability to deliver high-precision colored point maps. In In this experiment, we validate the high-precision mapping
contrast, FAST-LIVO managed to obtain the geometric structure capabilities of our system. To explore the mapping accuracy
but with completely blurred texture. R3LIVE struggles with both across different algorithms and ensure fairness, we compare our
geometric structure and texture clarity. Fig. 11 showcases tests system with FAST-LIO2, R3LIVE, and FAST-LIVO in scenes
in more complicated scenarios where LiDAR and/or camera characterized by rich texture and structured environments. We
both degenerate occasionally. The degeneration directions are take the sequences “SYSU 01,” “HKU Landmark,” and “CBD
indicated by respective arrows. “HKU Cultural Center” [see Building 01” as examples. Fig. S9 in the Supplementary Ma-
Fig. 11(a)] showcases the mapping results of FAST-LIVO2, terial [53] shows the colored point maps of these sequences
R3LIVE, and FAST-LIVO. As can be seen, R3LIVE and FAST- reconstructed in real time. We can clearly observe that the point
LIVO have distorted point maps, blurred textures, and drifts maps generated by FAST-LIVO2 retain the finest details among
exceeding 1 m. In contrast, FAST-LIVO2 successfully returns all the systems, the enlarged views of the colored point maps
to the starting point, achieving an impressive end-to-end error are akin to those in the actual RGB image. In the “SYSU
of less than 0.01 m, while achieving a consistent point map with 01” sequence, our algorithm produces fewer white noise dots
clear textures. “CBD Building 03” [see Fig. 11(b)] and “Mining on the signboard because we normalize the image colors to a
Tunnel” [see Fig. 11(c)] display only FAST-LIVO2 results, as reasonable exposure time using the recovered exposure time
R3LIVE and FAST-LIVO failed. In Fig. 11(b), the blue arrow before coloring, resulting in rarely overexposed colored point
represents movement toward a pure black screen, indicating maps. The reconstruction of the human and motorcycle in “CBD
concurrent LiDAR and camera degeneration. In Fig. 11(c1) and
(c2), the red points represent the LiDAR scan at that location, 10 [Link]

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
340 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

Fig. 11. Mapping results generated in real time in complex LiDAR degenerated and visually challenging scenes. (a), (b), and (c) correspond to “HKU Cultural
Center,” “CBD Building 03,” and “Mining Tunnel,” respectively. Different colored arrows indicate the directions of degeneration caused by different sensors (see
more details on YouTube).11

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 341

TABLE III
PROCESSING TIME (MS) PER LIDAR AND IMAGE FRAME

Fig. 12. Fully onboard UAV navigation algorithm flowchart.

Table III, our system exhibits the lowest processing time across
all sequences. The average computation time consumption on an
Intel i7 processor is only 30.03 ms (17.13 ms per LiDAR scan
and 12.90 ms per image frame), fulfilling real-time operation
at 10 Hz. Besides, our system can even operate in real time
on ARM processors with an average processing time per frame
of just 78.44 ms. LVI-SAM’s LiDAR and visual feature extrac-
tion modules in LIO and VIO are time-consuming. In addition
to the time consumed by LIO and VIO, LVI-SAM integrates
IMU preintegration constraints, visual odometry constraints,
and LiDAR odometry constraints within a factor graph, further
increasing the overall processing time. For R3LIVE, although
also employing a direct method, its pixelwise image alignment
necessitates the use of a large number of visual map points. In
contrast, our approach uses sparse points with reference patches,
enabling efficient alignment. In addition, R3LIVE maintains a
colored map that undergoes Bayesian updating, significantly in-
creasing the computational load as the map resolution increases.
For FAST-LIO2, the average processing time per frame (Table
S3 in the Supplementary Material [53] due to space constraints)
is approximately 10.35 ms less than FAST-LIVO2 due to not
processing additional image measurements.
FAST-LIVO2 also shows noticeable improvements over the
predecessor FAST-LIVO. The primary enhancement stems from
our application of inverse compositional formulation in the
sparse image alignment. Employing affine warping based on the
plane prior from LiDAR points further enhances the convergence
efficiency of our method. Consequently, FAST-LIVO2 reduces
the number of iterations per pyramid level from 10 to 3, while
still achieving superior accuracy.

Building 01” also exemplifies our ability to rebuild the details X. APPLICATIONS
of unstructured objects. In all sequences, the estimated final
position returns to the starting point with an end-to-end error of To showcase the superior performance and versatility of
less than 0.01 m. We also tested FAST-LIVO2 in the remaining FAST-LIVO2 in real-world applications, we develop multiple
sequences of the private dataset, with mapping results shown in solutions, including fully onboard autonomous UAV navigation,
Fig. S10– S13 in the Supplementary Material [53]. airborne mapping, textured mesh generation, and 3-D Gaussian
splatting (3DGS) reconstruction for 3-D scene representation.

E. Run Time Analysis A. Fully Onboard Autonomous UAV Navigation

In this section, we evaluate the average computational time per Given the high precision and robust localization performance
LiDAR scan and image frame of our proposed system, tested on of FAST-LIVO2, along with its real-time capabilities, we con-
a desktop PC equipped with an Intel i7-10700K CPU and 32GB duct closed-loop autonomous UAV flights.
RAM. Our evaluations span public datasets including Hilti’22,
Hilti’23, and NTU-VIRAL, and our private dataset. As shown in 11 [Link]

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
342 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

Fig. 15. (a) and (b) are the mesh and texture mapping of “CBD Building 01,”
respectively. (c) is the texture mapping of “Retail Street,” with (c1) and (c2)
showing local details.

Fig. 13. (a) and (b) are the enlarged point maps of the “Woods” and “Narrow
2) UAV Autonomous Navigation: We conduct four fully on-
Opening” experiments, respectively. The red points in (a1), (a3), and (b4) board autonomous UAV navigation experiments, “Basement,”
represent the current scan. (a2) and (a4) represent the first-person view at the “Woods,” “Narrow Opening,” and “SYSU Campus” (Table S2
corresponding locations. (b1), (b2), and (b3) depict the third-person view (see in the Supplementary Material [53]). “Basement” and “Woods”
more details on YouTube).12 experiments are fully autonomous flights incorporating all plan-
ning, MPC, and FAST-LIVO2 modules, while “Narrow Open-
ing” and “SYSU Campus” are manual flights with only MPC
and FAST-LIVO2 (without the planning component). As can be
seen, “Basement” and “Woods” showcase the UAV’s successful
autonomous navigation and obstacle avoidance. In “Narrow
Opening,” the UAV is commanded to fly close proximity to a wall
leading to few LiDAR points measurements. Nevertheless, the
raycasting module recalls a greater number of visual map points,
providing abundant constraints for localization, which allows for
stable localization. Moreover, “Basement” and “Narrow Open-
ing” experience LiDAR degeneration, observing only a single
wall [see Fig. 1(e1) and (e4), Fig. 13(b1)–(b4))], along with
Fig. 14. Processing time of each module and in total in UAV autonomous significant exposure variations [see Fig. 1(e5)–(e6)]. Despite
navigation experiments across “Basement,” “Woods,” “Narrow Opening,” and these challenges, our UAV system performed exceptionally well.
“SYSU Campus.” The MPC executes at 100 Hz while planning and FAST-
LIVO2 execute at 10 Hz, so its computation time is counted for ten times.
“Woods” involves the UAV moving at high speeds up to 3 m/s,
demanding rapid response from the entire UAV system [see
Fig. 13(a1)–(a4)]. “SYSU Campus”, a nondegenerated scene,
1) System Configurations: The hardware and software setup primarily demonstrates the onboard high-precision mapping
are illustrated in Fig. 12. For hardware, we use a NUC (Intel capabilities (see Fig. S14 in the Supplementary Material [53]).
i7-1360P CPU and 32 GB RAM) as the onboard computer. Finally, it is worth mentioning that in all these four UAV flights,
In terms of software, the localization component is powered severe lighting variation occurred. FAST-LIVO2 is able to esti-
by FAST-LIVO2, which provides position feedback at 10 Hz. mate exposure time that closely follows the ground-truth values
The localization result is fed to the flight controller to achieve (see Fig. S15 in the Supplementary Material [53]).
200 Hz feedback on position, velocity, and attitude. Besides Regarding onboard computational time, the need to run MPC
localization, FAST-LIVO2 supplies a dense registered point (at 100 Hz) and Planning (at 10 Hz) on the onboard computer
cloud to the planning module, the Bubble planner [55], which consumes computational resources and memory, limiting the
plans a smooth trajectory that is then tracked by an on-manifold computation resources available to FAST-LIVO2. Despite of the
model predictive control (MPC) [56]. The MPC calculates the concurrent execution of control and planning, as illustrated in
desired angular rates and thrust, which are tracked by respective Fig. 14, the average onboard processing time per LiDAR scan
low-level angular rate controllers running on the flight controller. and image frame for FAST-LIVO2, approximately 53.47 ms, is
Importantly, the MPC, Planner, and FAST-LIVO2 all operate on still well below the frame period 100 ms. The average processing
the onboard computer in real time. times for planning and MPC are 8.43 and 18.5 ms, respectively.
The total average processing time of 80.4 ms meets very well
12 [Link] the real-time requirements for onboard operations.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 343

Fig. 16. Comparison of ground-truth image, COLMAP+3DGS, and FAST-LIVO2+3DGS in terms of render details, computational time (time for generating
point clouds and estimating poses + training time), and PSNR for a random frame in “CBD Building 01” (see more details on YouTube.13 (a) Ground-truth image.
(b) COLMAP + 3DGS. (c) FAST-LIVO2 + 3DGS.

B. Airborne Mapping The dense color point clouds from FAST-LIVO2 can also
Airborne mapping represents a crucial task in surveying directly serve as the input of 3DGS. We conduct tests on the
and mapping applications. To evaluate the suitability of FAST- sequence “CBD Building 01” utilizing 300 frames out of a total
of 1180 images. The results are shown in Fig. 16. Compared
LIVO2 for this application, we conduct an aerial mapping
experiment using the public dataset MARS-LVIG [52] whose to COLMAP [59], our method significantly reduces the time
hardware configuration is detailed in Section VIII-A. We evalu- required to obtain dense point clouds and poses from 9 h to
21 s. However, the training time increases from 10 min 59 s to
ate the two sequences “HKairport01” and “HKisland01,” whose
real-time mapping results are illustrated in Fig. 1(a)–(c), with 15 min 30 s. This increase is attributed to the denser point clouds
Fig. 1(a) and (c) corresponding to “HKisland01,” and (b) depict- (downsampled to 5 cm), which introduce more parameters to
optimize. Nonetheless, the increased density and precision of
ing “HKairport01.” The results demonstrate the effectiveness of
FAST-LIVO2 in unstructured environments, such as forests and our point clouds result in a slightly higher peak signal-to-noise
islands. The system successfully captures many fine structures ratio (PSNR) compared to the PSNR obtained from COLMAP
inputs.
and sharp coloring effects, including buildings, lane marks on
roads, road curbs, tree crowns, and rocks, all of which are
clearly visible. The APE (RMSE) for these sequences are 0.64 XI. CONCLUSION AND FUTURE WORK
and 0.27 m for FAST-LIVO2, respectively, compared to 2.76 This article proposed FAST-LIVO2, a direct LIVO frame-
and 0.52 m for R3LIVE. The average processing times on the work achieving fast, accurate, and robust state estimation while
desktop PC (see Section IX-A), are approximately 25.2 and reconstructing the map on the fly. FAST-LIVO2 can achieve
21.8 ms, respectively, compared to 110.5 and 100.2 ms for high localization accuracy while being robust to severe LiDAR
R3LIVE. and/or visual degeneration.
The gain in speed is attributed to the use of raw LiDAR,
inertial, and camera measurements within an efficient ESIKF
C. Supporting 3-D Scene Applications: Mesh Generation, framework with sequential update. In the image update, an in-
Texture, and Gaussian Splatting verse compositional formulation along with a sparse patch-based
image alignment is further adopted to boost the efficiency. The
Leveraging the high-precision sensor localization and dense
gain in accuracy is attributed to the use (and even refine) of
3-D colored point map obtained from FAST-LIVO2, we develop
plane priors from LiDAR points to enhance accuracy of image
software applications for rendering pipelines including meshing
alignment. Besides, a single unified voxel map is used to manage
and texturing, as well as emerging NeRF-like rendering pipeline,
simultaneously the map points and the observed high-resolution
such as 3DGS. For meshing, we employ VDBFusion [57] based
image measurements. The voxel map structure, which supports
on the truncated signed distance function in “CBD Building 01,”
geometry construction and update, visual map point generation
as shown in Fig. 15(a). The sharp edges on the columns and the
and update, and reference patch update, is developed and val-
distinct structure of the roof are clearly visible, demonstrating
idated. The gain in robustness is due to real-time estimation
the high quality of the mesh. This level of detail is achieved
of exposure time, which effectively handles environment illu-
due to the high density of FAST-LIVO2’s point clouds and the
mination variation, and on-demand voxel raycasting to cope
exceptional accuracy of structural reconstruction. After mesh
with LiDARs’ close proximity blind zones. The efficiency and
construction, we use OpenMVS [58] to perform texture mapping
accuracy of FAST-LIVO2 were evaluated on extensive public
using the estimated camera poses in “CBD Building 01” and
datasets, while the robustness and effectiveness of each system
“Retail Street,” as shown in Fig. 15(b)–(c). In Fig. 15(c1)–(c2),
module were evaluated on private dataset. The applications
the texture images applied on the triangular facets are seamless
of FAST-LIVO2 in real-world robotics applications, such as
and accurately aligned, resulting in a highly clear and precise
texture mapping. This is attributed to pixel-level image align-
ment achieved by FAST-LIVO2. 13 [Link]

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
344 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

UAV navigation, 3-D mapping, and model rendering, were also [22] W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo visual inertial
demonstrated. lidar simultaneous localization and mapping,” in Proc. IEEE/RSJ Int. Conf.
As an odometry, FAST-LIVO2 may have drifts over long Intell. Robots Syst., 2019, pp. 370–377.
[23] J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth enhanced
distances. In the future, we could integrate loop closure and visual odometry,” Auton. Robots, vol. 41, pp. 31–43, 2017.
the sliding window optimization into FAST-LIVO2 to mitigate [24] J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: Lidar-monocular vi-
this long-term drift. Moreover, the accurate and dense colored sual odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018,
point maps could be used to extract semantic information for pp. 7872–7879.
[25] Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “CamVox: A low-cost
object-level semantic mapping. and accurate lidar-assisted visual SLAM system,” in Proc. IEEE Int. Conf.
Robot. Automat., 2021, pp. 5049–5055.
[26] S.-S. Huang, Z.-Y. Ma, T.-J. Mu, H. Fu, and S.-M. Hu, “Lidar-monocular
visual odometry using point and line features,” in Proc. IEEE Int. Conf.
REFERENCES Robot. Automat., 2020, pp. 1091–1097.
[27] C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D.
[1] R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source SLAM Tardós, “ORB-SLAM3: An accurate open-source library for visual,
system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robot., visual–inertial, and multimap SLAM,” IEEE Trans. Robot., vol. 37, no. 6,
vol. 33, no. 5, pp. 1255–1262, Oct. 2017. pp. 1874–1890, Dec. 2021.
[2] J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE [28] Y.-S. Shin, Y. S. Park, and A. Kim, “DVL-SLAM: Sparse depth enhanced
Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018. direct visual-lidar SLAM,” Auton. Robots, vol. 44, no. 2, pp. 115–130,
[3] J. Engel, T. Schöps, and D. Cremers, “LSD-SLAM: Large-scale direct 2020.
monocular SLAM,” in Proc. Eur. Conf. Comput. Vis., 2014, pp. 834–849. [29] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “LIC-Fusion: Lidar-
[4] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, inertial-camera odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots
“SVO: Semidirect visual odometry for monocular and multicamera sys- Syst., 2019, pp. 5848–5854.
tems,” IEEE Trans. Robot., vol. 33, no. 2, pp. 249–265, Apr. 2017. [30] K. Sun et al., “Robust stereo visual inertial odometry for fast autonomous
[5] J. Zhang and S. Singh, “LOAM: Lidar odometry and mapping in real-time,” flight,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 965–972, Apr. 2018.
in Proc. Conf. Robot.: Sci. Syst., vol. 2, no. 9, 2014. [31] X. Zuo et al., “LIC-Fusion 2.0: Lidar-inertial-camera odometry with
[6] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar sliding-window plane-feature tracking,” in Proc. IEEE/RSJ Int. Conf.
odometry and mapping package for lidars of small FOV,” in Proc. IEEE Intell. Robots Syst., 2020, pp. 5112–5119.
Int. Conf. Robot. Automat., 2020, pp. 3126–3131. [32] D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multi-
[7] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized modal landmark tracking for tightly coupled lidar-visual-inertial odom-
lidar odometry and mapping on variable terrain,” in Proc. IEEE/RSJ Int. etry,” IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 1004–1011,
Conf. Intell. Robots Syst., 2018, pp. 4758–4765. Apr. 2021.
[8] C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “FAST-LIVO: [33] J. Lin, C. Zheng, W. Xu, and F. Zhang, “R2Live: A robust, real-time,
Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry,” in lidar-inertial-visual tightly-coupled state estimator and mapping,” IEEE
Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 4003–4009. Robot. Automat. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021.
[9] T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monoc- [34] B. M. Bell and F. W. Cathey, “The iterated Kalman filter update as a Gauss-
ular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, Newton method,” IEEE Trans. Autom. Control, vol. 38, no. 2, pp. 294–297,
pp. 1004–1020, Aug. 2018. Feb. 1993.
[10] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A [35] T. Shan, B. Englot, C. Ratti, and D. Rus, “LVI-SAM: Tightly-coupled
versatile and accurate monocular SLAM system,” IEEE Trans. Robot., lidar-visual-inertial odometry via smoothing and mapping,” in Proc. IEEE
vol. 31, no. 5, pp. 1147–1163, Oct. 2015. Int. Conf. Robot. Automat., 2021, pp. 5692–5698.
[11] M. Irani and P. Anandan, “All about direct methods,” in Proc. Workshop [36] J. Lin and F. Zhang, “R 3 live: A robust, real-time, RGB-colored, lidar-
Vis. Algorithms, Theory Pract., 1999, pp. 267–277. inertial-visual tightly-coupled state estimation and mapping package,” in
[12] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monoc- Proc. Int. Conf. Robot. Automat., 2022, pp. 10672–10678.
ular visual odometry,” in Proc. IEEE Int. Conf. Robot. Automat., 2014, [37] J. Lin and F. Zhang, “R 3 Live++: A robust, real-time, radiance reconstruc-
pp. 15–22. tion package with a tightly-coupled lidar-inertial-visual state estimator,”
[13] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct lidar- IEEE Trans. Pattern Anal. Mach. Intell., 2024.
inertial odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, [38] J. Engel, V. Usenko, and D. Cremers, “A photometrically calibrated
Aug. 2022. benchmark for monocular visual odometry,” 2016, arXiv:1607.02555.
[14] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and [39] W. Wang, J. Liu, C. Wang, B. Luo, and C. Zhang, “DV-Loam: Direct
probabilistic adaptive voxel mapping for accurate online lidar odom- visual lidar odometry and mapping,” Remote Sens., vol. 13, no. 16, 2021,
etry,” IEEE Robot. Automat. Lett., vol. 7, no. 3, pp. 8518–8525, Art. no. 3340.
Jul. 2022. [40] Z. Yuan, Q. Wang, K. Cheng, T. Hao, and X. Yang, “SDV-Loam: Semi-
[15] M. Meilland, A. I. Comport, and P. Rives, “Real-time dense visual tracking direct visual-lidar odometry and mapping,” IEEE Trans. Pattern Anal.
under large lighting variations,” in Proc. Brit. Mach. Vis. Conf. Brit. Mach. Mach. Intell., vol. 45, no. 9, pp. 11203–11220, Sep. 2023.
Vis. Assoc., 2011, pp. 45–1. [41] H. Zhang, L. Du, S. Bao, J. Yuan, and S. Ma, “LVIO-Fusion:tightly-
[16] T. Tykkälä, C. Audras, and A. I. Comport, “Direct iterative closest point coupled lidar-visual-inertial odometry and mapping in degenerate envi-
for real-time visual odometry,” in Proc. IEEE Int. Conf. Comput. Vis. ronments,” IEEE Robot. Automat. Lett., vol. 9, no. 4, pp. 3783–3790,
Workshops, 2011, pp. 2050–2056. Apr. 2024.
[17] C. Kerl, J. Sturm, and D. Cremers, “Robust odometry estimation for RGB- [42] W. Xu and F. Zhang, “FAST-LIO: A fast, robust lidar-inertial odometry
D cameras,” in Proc. IEEE Int. Conf. Robot. Automat., 2013, pp. 3748– package by tightly-coupled iterated Kalman filter,” IEEE Robot. Automat.
3754. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.
[18] J. Engel, J. Sturm, and D. Cremers, “Semi-dense visual odometry for [43] D. He, W. Xu, and F. Zhang, “Symbolic representation and toolkit de-
a monocular camera,” in Proc. IEEE Int. Conf. Comput. Vis., 2013, velopment of iterated error-state extended Kalman filters on manifolds,”
pp. 1449–1456. IEEE Trans. Ind. Electron., vol. 70, no. 12, pp. 12533–12544, Dec. 2023.
[19] K. Chen, R. Nemiroff, and B. T. Lopez, “Direct lidar-inertial odometry: [44] D. Willner, C.-B. Chang, and K.-P. Dunn, “Kalman filter algorithms for a
Lightweight LIO with continuous-time motion correction,” in Proc. IEEE multi-sensor system,” in Proc. IEEE Conf. Decis. Control Including 15th
Int. Conf. Robot. Automat., 2023, pp. 3983–3989. Symp. Adaptive Processes, 1976, pp. 570–574.
[20] Z. Wang, L. Zhang, Y. Shen, and Y. Zhou, “D-LIOM: Tightly-coupled [45] J. Ma and S. Sun, “Globally optimal distributed and sequential state fusion
direct lidar-inertial odometry and mapping,” IEEE Trans. Multimedia, filters for multi-sensor systems with correlated noises,” Inf. Fusion, vol. 99,
vol. 25, pp. 3905–3920, 2023. 2023, Art. no. 101885.
[21] J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping [46] Y. Ren, Y. Cai, F. Zhu, S. Liang, and F. Zhang, “ROG-MAP: An efficient
with high robustness and low drift,” J. Field Robot., vol. 35, no. 8, robocentric occupancy grid map for large-scene and high-resolution lidar-
pp. 1242–1264, 2018. based motion planning,” 2023, arXiv:2302.14819.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
ZHENG et al.: FAST-LIVO2: FAST, DIRECT LIDAR–INERTIAL–VISUAL ODOMETRY 345

[47] R. M. Stereopsis, “Accurate, dense, and robust multiview stereopsis,” Zuhao Zou received the [Link]. degree in automatic
IEEE Trans. Pattern Anal. Mach. Intell., vol. 32, no. 8, pp. 1362–1376, control system engineering from The University of
Aug. 2010. Sheffield, Sheffield, U.K., in 2016, and the [Link].
[48] S. Baker and I. Matthews, “Lucas-Kanade 20 years on: A unifying frame- degree in computer vision, graphic, and imaging from
work,” Int. J. Comput. Vis., vol. 56, pp. 221–255, 2004. University College London, London, U.K., in 2017.
[49] T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, “NTU He is currently working toward the Ph.D. degree in
VIRAL: A visual-inertial-ranging-lidar dataset, from an aerial vehicle mechanical engineering with Hong Kong University,
viewpoint,” Int. J. Robot. Res., vol. 41, no. 3, pp. 270–280, 2022. Hong Kong, China.
[50] M. Helmberger, K. Morin, B. Berner, N. Kumar, G. Cioffi, and D. Scara- His research interests include robotics, sensor fu-
muzza, “The Hilti SLAM challenge dataset,” IEEE Robot. Automat. Lett., sion, localization and mapping, and loop detection.
vol. 7, no. 3, pp. 7518–7525, Jul. 2022.
[51] L. Zhang et al., “Hilti-Oxford dataset: A millimeter-accurate benchmark
for simultaneous localization and mapping,” IEEE Robot. Automat. Lett.,
vol. 8, no. 1, pp. 408–415, Jan. 2023.
[52] H. Li et al., “MARS-LVIG dataset: A multi-sensor aerial robots SLAM
dataset for lidar-visual-inertial-GNSS fusion,” Int. J. Robot. Res., vol. 43, Tong Hua received the B.S. degree in electronic engi-
no. 8, 2024, Art. no. 02783649241227968. neering from Shanghai Jiao Tong University, Shang-
[53] “Supplementary material: Fast-livo2: Fast, direct lidar-inertial-visual hai, China, in 2021. He has been working toward the
odometry,” Aug. 2024. [Online]. Available: [Link] master’s degree in information and communication
FAST-LIVO2/blob/main/Supplementary/LIVO2_supplementary.pdf engineering with the Shanghai Key Laboratory of
[54] C. Klug, C. Arth, D. Schmalstieg, and T. Gloor, “Measurement uncertainty Navigation and Location Based Services, Shanghai
analysis of a robotic total station simulation,” in Proc. IECON 44th Annu. Jiao Tong University, Shanghai, China, since 2021.
Conf. IEEE Ind. Electron. Soc., 2018, pp. 2576–2582. His research interests include multisensor fusion
[55] Y. Ren et al., “Bubble planner: Planning high-speed smooth quadrotor and visual inertial odometry.
trajectories using receding corridors,” in Proc. IEEE/RSJ Int. Conf. Intell.
Robots Syst., 2022, pp. 6332–6339.
[56] G. Lu, W. Xu, and F. Zhang, “On-manifold model predictive control for
trajectory tracking on robotic systems,” IEEE Trans. Ind. Electron., vol. 70,
no. 9, pp. 9192–9202, Sep. 2023.
[57] I. Vizzo, T. Guadagnino, J. Behley, and C. Stachniss, “VDBFusion: Flexi- Chongjian Yuan received the [Link]. degree in au-
ble and efficient TSDF integration of range sensor data,” Sensors, vol. 22, tomation from the College of Control Science and
no. 3, 2022, Art. no. 1296. [Online]. Available: [Link] Engineering, Zhejiang University, Hangzhou, China,
1424-8220/22/3/1296 in 2016. He is currently working toward the Ph.D.
[58] D. Cernea, “OpenMVS: Multi-view stereo reconstruction library 2020,” degree in robotics with the Department of Mechanical
2020. [Online]. Available: [Link] Engineering, The University of Hong Kong, Hong
[59] J. L. Schönberger and J.-M. Frahm, “Structure-from-motion revisited,” in Kong, China.
Proc. IEEE Conf. Comput. Vis. Pattern Recognit., 2016, pp. 4104–4113. His research interests include light detection and
ranging SLAM and sensor fusion.

Dongjiao He (Member, IEEE) received the B.S. de-

Chunran Zheng (Student Member, IEEE) received gree from Southeast University, Nanjing, China, in
the [Link]. degree in automation from Xian Jiaotong 2016, the M.S. degree from Shanghai Jiao Tong Uni-
University, Xi’an, China, in 2020. He is currently versity, Shanghai, China, in 2019, both in mechanical
working toward the Ph.D. degree in robotics and engineering, and the Ph.D. degree in robotics from the
SLAM with the Department of Mechanical Engi- University of Hong Kong, Hong Kong, China, in July
neering, The University of Hong Kong, Hong Kong, 2024, under supervision of Prof. Fu Zhang.
China. Her research interests of the doctoral degree
His research interests include multisensor calibra- include advanced multisensor algorithms for au-
tion, sensor fusion, LiDAR–inertial–visual SLAM, tonomous navigation, SLAM, and other related fields
and 3-D Gaussian splatting. of UAVs. She is committed to advancing research for
autonomous systems, including multisource information fusion, autonomous
perception, and strategy decision.

Bingyang Zhou (Member, IEEE) received the [Link].

Wei Xu received the Ph.D. degree in mechanical
degree in automation from the Harbin Institute of
engineering from the University of Hong Kong, Hong
Technology, Shenzhen, China, in 2023. He is cur-
Kong, China, in 2022. rently working toward the [Link]. degree in me-
He is currently the Chief Technology Officer with
chanical engineering with MaRS Lab, Department
Manifold Tech Limited, Hong Kong, China. His re-
of Mechanical Engineering, The University of Hong
search and expertise include filtering algorithms, mul-
Kong, Hong Kong, China.
tisensor fusion, simultaneous localization and dense His research focuses on simultaneous localization
mapping, unmanned aerial vehicle systems, and mo-
and mapping.
tion planning.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.
346 IEEE TRANSACTIONS ON ROBOTICS, VOL. 41, 2025

Zheng Liu (Member, IEEE) received the [Link]. Rong Wang received the [Link]. degree in automation
degree in automation from the Harbin Institute of from Beihang University, Beijing, China, 2013, and
Technology, Harbin, China, in 2019, and the Ph.D. the Ph.D. degree in automation from the University
degree in mechanical engineering from the University of Chinese Academy of Sciences, Beijing, China, in
of Hong Kong, Hong Kong, China, in 2024. June 2018.
His research interests include visual or LiDAR- Her Ph.D. work focused on SLAM and augmented
based localization and mapping, and sensor fusion reality. From July 2018, she was with the Informa-
and calibration. tion Science Academy, China Electronics Technol-
ogy Group Corporation, Beijing, China. Her research
interests include semantic SLAM and UAV.

Fanle Meng received the [Link]. degree in biomed-

Jiarong Lin (Member, IEEE) received the B.S. de- ical engineering from the Huazhong University of
gree in optical information science and technology Science and Technology, Wuhan, China, in 2010,
from the University of Electronic Science and Tech- and the Ph.D. degree in biomedical engineering from
nology of China, Chengdu, China, in 2015. He is Tsinghua University, Beijing, China, in June 2018.
currently working toward the Ph.D. degree in robotics Her Ph.D. work focused on medical robotics and
with the Department of Mechanical Engineering, The surgery navigation. From July 2018, she was with
University of Hong Kong, Hong Kong, China. the Information Science Academy, China Electronics
His research interests include light detection and Technology Group Corporation, Beijing, China. Her
ranging mapping and sensor fusion. research interests include SLAM and UGV.

Fu Zhang (Member, IEEE) received the B.E. degree

in automation from the University of Science and
Technology of China, Hefei, China, in 2011, and
the Ph.D. degree in controls from the University of
Fangcheng Zhu (Student Member, IEEE) received California, Berkeley, CA, USA, in 2015.
the [Link]. degree in automation from the School His Ph.D. work focused on self-calibration and
of Mechanical Engineering and Automation, Harbin control of microrate-integrating gyrosensors. In 2016,
Institute of Technology, Shenzhen, China, in 2021. he was a Research Assistant Professor in design and
He is currently working toward the Ph.D. degree control of unmanned aerial vehicles (UAVs) with the
in robotics with the Department of Mechanical En- Robotics Institute, Hong Kong University of Science
gineering, University of Hong Kong, Hong Kong, and Technology, Hong Kong, China. From Aug 2018,
China. he was an Assistant Professor with the Department of Mechanical Engineering,
His research interests include LiDAR-based simul- University of Hong Kong, Hong Kong, China. His research interests include
taneous localization and mapping, sensor calibration, robotics and controls, with focus on UAV design, navigation, control, and
and aerial swarm systems. lidar-based simultaneous localization and mapping.

Yunfan Ren (Student Member, IEEE) received the

[Link]. degree in automation from the Harbin Institute
of Technology, Harbin, China, in 2021. He is currently
working toward the Ph.D. degree with the Department
of Mechanical Engineering, The University of Hong
Kong, Hong Kong, China.
He is a Member of the MaRS Lab, Hong Kong,
China. His research interests include autonomous
navigation, UAV planning, optimal control, and
swarm system autonomy.
Mr. Ren was recognized as an Outstanding Naviga-
tion Paper Finalist at ICRA 2023 and as both the Best Overall and Best Student
Paper Finalist at IROS 2023.

Authorized licensed use limited to: Shanghai Dianji University. Downloaded on August 01,2025 at 03:25:31 UTC from IEEE Xplore. Restrictions apply.