Measurement-level LiDAR-Inertial-Visual Odometry (LIVO) can achieve high accuracy, and its practical resource requirements depend on the adopted association and map-maintenance strategy. Persistently attaching dense photometric patches or appearance attributes to mapped LiDAR points can introduce additional storage, association, and update costs. This paper presents F-LIVO, a filter-based LIVO framework that couples a visual-inertial subsystem and a LiDAR-inertial subsystem through feature-level point-to-plane constraints between visual points and LiDAR planes. To preserve cross-modal coupling when visual triangulation is weak, we further recover visual feature depth from the LiDAR map through local plane fitting and ray-plane intersection. In the LiDAR update, the covariance of each point-to-plane residual is adaptively scaled according to local planar reliability, thereby reducing overconfident updates on poorly conditioned surfaces. Because the proposed method relies on sparse geometric features rather than persistent dense photometric map attributes, it retains a compact map representation. Experiments on multiple public datasets show competitive, sequence-dependent accuracy. Across the evaluated degradation cases, F-LIVO maintains continuous estimation with comparatively low accumulated end-to-end drift. Under the evaluated resource configuration, it achieves the lowest mean and peak central processing unit (CPU) usage, the lowest peak resident set size (RSS), and the lowest RSS growth rate among the evaluated implementations, while R2LIVE retains a lower mean RSS. The source code will be made publicly available upon acceptance to facilitate reproducible research.
Figures & tables
Notations
Explanation
⊞/⊟
The ⊞ and ⊟ operators on the state manifold
(⋅)G
A vector (⋅) expressed in the global frame
(⋅)C
A vector (⋅) expressed in the camera frame
(⋅)I
A vector (⋅) expressed in the IMU frame
(⋅)L
A vector (⋅) expressed in the LiDAR frame
TLI
Rigid extrinsic transformation from the LiDAR frame to the IMU frame
TABLE I: Important Notations
Fig. 1: Overview of the proposed system. The system integrates LiDAR, IMU, and camera measurements within a unified filtering framework for LIVO. LiDAR and visual observations are coupled through point-cloud geometric information to achieve feature-level fusion. The resulting updates are applied to the relevant states in the filter, and the final output is the fused odometry estimate.
Fig. 2: Illustrations of the visual-plane construction and LiDAR-assisted depth recovery. (a) Triangulated visual feature points. (b) Delaunay triangulation built from the tracked visual features. (c) Coplanar aggregation of neighboring triangles into visual feature planes. (d) LiDAR points projected onto the image plane are used for local search and plane fitting to recover feature depth when visual triangulation is weak. (e) Red points denote visual feature points whose depths are recovered from LiDAR geometry.
Fig. 3: Geometric illustration of the cross-modal coplanar association used in the camera update. Orange points denote LiDAR points and purple points denote visual feature points. When the two sets are associated with the same local plane, point-to-LiDAR-plane and LiDAR-point-to-visual-plane constraints can be constructed consistently.
Fig. 4: Illustrations of the adaptive point-to-plane covariance and the unified residual construction. (a) Ideal point-to-plane distance constraint, where the point is assumed to lie on the plane. (b) Practical point-to-plane constraint, where the fitted plane has thickness and the point is generally not exactly on the plane. (c) Unified residual model used for feature-level LiDAR-visual fusion, where the blue and purple rays denote points associated with different plane features.
Fig. 5: (a) is the construction environment, rich in planar structure. (b) shows the campus environment, filled with piles of concrete and gravel. Both (a) and (b) have overexposure. (c) and (d) are basement environments with narrow passages and poor lighting conditions. (e) and (f) correspond to long-sequence scenarios.
R3LIVE
R2LIVE
FAST-LIVO2
FAST-LIO2
ov_plane
F-LIVO (w/)
F-LIVO (w/o)
Basement1
17.5/31.2
19.9/29.5
15.8 /26.6
15.8/27.0
–/–
15.8 /28.0
15.9/28.3
Basement4
73.6/181.3
10.0/20.9
9.40/17.8
10.6/25.3
50.5/136.0
6.04 /11.5
57.2/126.1
Campus2
6.2/10.7
5.2/10.2
4.2 /10.1
7.0/16.8
62.4/90.1
5.7/9.7
8.0/17.7
Construction2
9.7/ 19.8
10.0/20.9
9.8/ 13.9
12.4/26.5
261.3/1054.1
8.2 /25.1
20.2/28.4
LabSurvey2
–/–
–/–
–/–
–/–
–/–
5.5 /25.8
–/–
UzhArea2
22.8 /38.7
26.4/43.5
23.6/39.7
29.8/48.5
–/–
23.5/42.4
26.8/46.0
TABLE II: The RMSE/MAX of ATE (UNIT: cm) results in HILTI and i2Nav-Robot dataset. The best results are marked in bold.
Fig. 6: Representative trajectory estimates on Basement4 for the compared methods. The repeated-evaluation ATE RMSE/MAX is reported in Table II .
Fig. 7: Representative trajectory estimates for F-LIVO and FAST-LIVO2 on Basement4 . The repeated-evaluation ATE RMSE/MAX is reported in Table II .
Fig. 8: Evolution of the planar quality factor σplane on the Basement4 sequence.
Fig. 9: F-LIVO’s trajectory in Building .
Fig. 10: Results on degenerate sequences 01 and 02 show that F-LIVO maintains continuous estimation with comparatively low accumulated end-to-end drift under the evaluated concurrent visual and LiDAR degradation.
Fig. 11: (a) R3LIVE partially recovers structure with slight misalignment. (b) R2LIVE exhibits pronounced distortion. (c) F-LIVO preserves the sharp-corner structure. (d) FAST-LIVO2 fails to follow the corner turn when the LiDAR faces the blank wall, resulting in an incomplete map.
Sequence
FAST-LIVO2
R3LIVE
R2LIVE
F-LIVO (w/o AC)
F-LIVO
Degenerate seq00
570.0/20.06
5.72/0.38
23.42/1.43
7.113/1.695
7.52/0.39
Degenerate seq01
1300.6/83.64
3.54/0.19
–/–
9.768/1.012
4.66/0.25
Degenerate seq02
–/–
8.46/0.28
–/–
21.947/2.251
13.68/0.23
LiDAR Degenerate
–/–
–/–
–/–
18.7/1.32
12.63/0.82
Visual Challenge
–/–
–/–
–/–
7.9/0.41
6.20/0.26
TABLE III: Accumulated end-to-end translation/rotation drift without loop closure (cm/deg). F-LIVO (w/o AC) denotes the fixed-noise variant without adaptive covariance weighting.
Fig. 12: Qualitative continuity evaluation in a tunnel-like degenerate environment. The red box marks the long, narrow, and repetitive tunnel segment, and the enlarged view shows the corresponding local reconstruction. The pose output and reconstruction remain continuous through the highlighted segment. Since no complete ground-truth trajectory is available, the figure provides qualitative evidence of estimator continuity rather than trajectory-wide accuracy.
FAST-LIVO2
F-LIVO
R3LIVE
Drift (m/deg)
2.01 / 0.77
2.03 / 0.74
2.19 / 1.89
TABLE IV: Odometry drift in large-scale environment.
Fig. 13: Process-level resource usage on hku_campus_seq_00 under the unified profiling protocol. (a) Aggregate process CPU usage, where 100% corresponds to one logical core. (b) Resident set size (RSS). All methods are evaluated over the same 170-s interval after excluding the first 10 s for initialization.
Method
Mean CPU (%)
Peak CPU (%)
Mean RSS (GB)
Peak RSS (GB)
Growth rate (MB/min)
F-LIVO
104.99
137.65
1.250
1.322
36.06
FAST-LIVO2
243.80
332.87
3.052
5.679
1909.48
R3LIVE
331.52
425.49
2.611
4.742
1501.62
R2LIVE
206.35
248.57
0.976
1.451
318.49
TABLE VI: Process-level resource statistics over the common 170-s analysis interval. CPU is aggregate process CPU, where 100% corresponds to one logical core; memory is process RSS.
State Size
15
30
60
120
R3LIVE
0.034
0.067
0.209
0.998
FAST-LIVO2
0.023
0.024
0.026
0.028
F-LIVO
0.024
0.024
0.025
0.026
TABLE V: MEASUREMENT-UPDATE TIME UNDER DIFFERENT STATE DIMENSIONS (MS).
Light detection and ranging (LiDAR)-inertial-visual odometry fuses geometric depth with visual measurements, but its sensors fail independently: LiDAR in under-constrained geometry, vision under poor illumination or texture. Weak information is harmless, since the filter then relies on the other sensor or the inertial prediction; over-confident information is not. Existing countermeasures act on weak information or switch vision by hand-set thresholds. This article presents SA-LIVO, which fuses vision through explicit, online-estimated error models. A frame-level noise scale, estimated by covariance matching, absorbs whole-image errors, and Schmidt filtering carries the correlation of reused observations. Subspace-aware information fusion models the error shared by a frame's observations as a pose offset capping the visual information per direction; the exact fusion attenuates vision only above that cap and reduces to the information sum when the visual model is consistent. LiDAR and visual residuals share one invariant extended Kalman filter loop. On 24 handheld and aerial sequences, SA-LIVO is the most accurate or tied on 22 and stays bounded where others diverge; on the HILTI 2022 benchmark it needs 13.4 ms per frame on a laptop processor and 2.0-6.2x less memory than FAST-LIVO2, R3LIVE, and SR-LIVO.
Yinong Cao, Xin He, Yijie Wang +11
Key Laboratory of Space Active Opto-Electronics Technology, Shanghai Institute of Technical Physics, Chinese Academy of Sciences, Shanghai 200083, China. · Hangzhou Institute for Advanced Study, University of Chinese Academy of Sciences, Hangzhou 310024, China.
Real-time LiDAR-visual-inertial odometry and mapping is crucial for navigation and planning tasks in intelligent transportation systems. This study presents a pose-only bundle adjustment (PA) LiDAR-visual-inertial odometry (LVIO), named PA-LVIO, to meet the urgent need for real-time navigation and mapping. The proposed PA framework for LiDAR and visual measurements is highly accurate and efficient, and it can derive reliable frame-to-frame constraints within multiple frames. A marginalization-free and frame-to-map (F2M) LiDAR measurement model is integrated into the state estimator to eliminate odometry drifts. Meanwhile, an IMU-centric online spatial-temporal calibration is employed to obtain a pixel-wise LiDAR-camera alignment. With accurate estimated odometry and extrinsics, a high-quality and RGB-rendered point-cloud map can be built. Comprehensive experiments are conducted on both public and private datasets collected by wheeled robot, unmanned aerial vehicle (UAV), and handheld devices with 28 sequences and more than 50 km trajectories. Sufficient results demonstrate that the proposed PA-LVIO yields superior or comparable performance to state-of-the-art LVIO methods, in terms of the odometry accuracy and mapping quality. Besides, PA-LVIO can run in real-time on both the desktop PC and the onboard ARM computer. The codes and datasets are open sourced on GitHub (https://github.com/i2Nav-WHU/PA-LVIO) to benefit the community.
Hailiang Tang, Tisheng Zhang, Liqiang Wang +3
the GNSS Research Center, Wuhan University, Wuhan 430079, China · the Hubei Technology Innovation Center for Spatiotemporal Information and Positioning Navigation, Wuhan 430079, China · the Hubei Luojia Laboratory, Wuhan 430079, China
Robust state estimation and mapping in long-term, large-scale, and highly dynamic environments remains a key challenge in robotics. Existing LiDAR-Inertial-Visual Odometry (LIVO) systems achieve strong local accuracy but suffer from accumulated drift over long distances and may fail in geometrically degraded or textureless scenes. Meanwhile, GNSS-aided fusion frameworks often rely on LiDAR or visual odometry for state prediction and outlier rejection, making them vulnerable when odometry degenerates. To address these limitations, we propose a tightly coupled LiDAR-Inertial-Visual-GNSS fusion framework based on an Error-State Iterated Kalman Filter. An online spatiotemporal alignment module using Dynamic Time Warping is introduced for highly dynamic conditions. To better exploit GNSS precision, we develop observation models based on Doppler shifts and fixed-anchor Time-Differenced Carrier Phase, providing millimeter-level relative constraints without augmenting historical anchor states. We further design a degeneracy-aware dual-mode outlier rejection strategy that switches between LIVO-prior-guided rejection and GNSS-aided recovery according to the LIVO degeneracy level. Experiments on the public M3DGR dataset and a custom 20~m/s fixed-wing UAV dataset demonstrate that our system reduces accumulated drift and map ghosting, outperforming state-of-the-art methods in accuracy and robustness.
Zhiyu Chen, Chunran Zheng, Jiayu Wen +4
College of Mechatronics and Control Engineering, Shenzhen University, Shenzhen 518060, China · Department of Mechanical Engineering, The University of Hong Kong, Hong Kong SAR, China · College of Automation, Harbin Engineering University, Harbin 150001, China