pith. sign in

arxiv: 2606.25699 · v1 · pith:IU7YD7C3new · submitted 2026-06-24 · 💻 cs.RO

SA-LIVO: Efficient LiDAR-Inertial-Visual Odometry with Subspace-Aware Degeneracy Handling

Yinong Cao , Xin He , Yuwei Chen , Shijie Liu , Chunlai Li , Jianyu Wang This is my paper

Pith reviewed 2026-06-25 21:09 UTC · model grok-4.3

classification 💻 cs.RO
keywords LiDAR-visual-inertial odometrydegeneracy handlingsubspace-aware fusioninformation matrixInEKFsensor fusionSLAMrobot localization
0
0 comments X

The pith

Eigendecomposition of the joint LiDAR-visual information matrix with per-direction soft gates allows selective compensation only in degenerate directions during odometry.

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

The paper argues that current LiDAR-visual-inertial systems fail when they handle degeneracy at the full-modality level, letting visual residuals leak into well-constrained directions or disperse inefficiently. SA-LIVO instead eigendecomposes the combined information matrix and applies a linear-clamp soft gate to each eigendirection so that visual data strengthens only the deficient axes while leaving observable ones untouched. Residuals from both sensors are then solved together inside one InEKF iteration at a common linearization point, with photometric Jacobians computed once and reused. This produces competitive accuracy on three public benchmarks plus concurrent-degradation tests, plus lower runtime and memory than iterated baselines.

Core claim

The Subspace-Aware Information Fusion framework eigendecomposes the joint LiDAR-visual information matrix and applies a linear-clamp soft gate per eigendirection, attenuating degenerate directions while preserving observable ones at full strength. LiDAR and visual residuals are then jointly optimized in one InEKF loop at a shared linearization point. Photometric Jacobians are assembled once before the loop and reused across iterations.

What carries the argument

Subspace-Aware Information Fusion (SAIF), which eigendecomposes the joint information matrix and applies a direction-specific linear-clamp soft gate to control how much each sensor contributes to each pose axis.

If this is right

  • Accuracy remains competitive with the strongest existing LIVO baselines on the HILTI'22, New College, and Oxford Spires sequences.
  • Drift stays bounded in concurrent LiDAR-visual degradation cases where competing systems lose track.
  • Joint optimization inside a single InEKF loop with reused Jacobians yields 12.3 ms per frame on a laptop CPU and 26.8 ms on an embedded ARM board.
  • Peak memory is 3.6-6.3 times lower than iterated-filter baselines.

Where Pith is reading between the lines

These are editorial extensions of the paper, not claims the author makes directly.

  • The same eigendirection gating could be applied to other multi-sensor combinations that face independent failure modes, such as radar-visual fusion.
  • Reusing Jacobians across iterations may allow the same accuracy at even lower update rates on power-constrained platforms.
  • Explicitly measuring the angle between successive eigendirections across frames could expose when the soft-gate assumption begins to break.

Load-bearing premise

The eigendirections extracted from the joint information matrix cleanly separate the observable and degenerate subspaces without creating misalignment between the linearization point and the actual residuals.

What would settle it

A controlled test sequence in which SA-LIVO produces larger drift or divergence than a binary degeneracy detector once LiDAR scan geometry becomes under-constrained in specific directions while visual features remain available.

Figures

Figures reproduced from arXiv: 2606.25699 by Chunlai Li, Jianyu Wang, Shijie Liu, Xin He, Yinong Cao, Yuwei Chen.

Figure 1
Figure 1. Figure 1: Representative mapping results of SA-LIVO across diverse environments. [PITH_FULL_IMAGE:figures/full_fig_p002_1.png] view at source ↗
Figure 2
Figure 2. Figure 2: System overview of SA-LIVO. LiDAR, camera, and IMU streams enter from the top; IMU measurements drive continuous state propagation (Sect. [PITH_FULL_IMAGE:figures/full_fig_p004_2.png] view at source ↗
Figure 3
Figure 3. Figure 3: Planarity-aware voxel freezing. As LiDAR points accumulate within [PITH_FULL_IMAGE:figures/full_fig_p006_3.png] view at source ↗
Figure 4
Figure 4. Figure 4: Hierarchical voxel search for geometric constraint association. The [PITH_FULL_IMAGE:figures/full_fig_p008_4.png] view at source ↗
Figure 5
Figure 5. Figure 5: Information-efficient direct photometric VIO pipeline. LiDAR-anchored map points are projected and matched photometrically; Jacobians are frozen [PITH_FULL_IMAGE:figures/full_fig_p009_5.png] view at source ↗
Figure 6
Figure 6. Figure 6: LiDAR-guided sparse photometric sampling. A LiDAR map point [PITH_FULL_IMAGE:figures/full_fig_p009_6.png] view at source ↗
Figure 7
Figure 7. Figure 7: Subspace-Aware Information Fusion via the linear-clamp soft gate. [PITH_FULL_IMAGE:figures/full_fig_p011_7.png] view at source ↗
Figure 8
Figure 8. Figure 8: Handheld data-collection platform used for the self-collected dataset. [PITH_FULL_IMAGE:figures/full_fig_p014_8.png] view at source ↗
Figure 9
Figure 9. Figure 9: Qualitative mapping comparison on the indoor-chairs sequence from our self-collected dataset. (a1)–(a2) Two viewpoints of the complete SA-LIVO colored point cloud map; (a3) close-up of the pillar-and-chair region. (b1)–(b4) The same region from FAST-LIVO, FAST-LIVO2, SR-LIVO, and R3LIVE, respectively; SR-LIVO and R3LIVE fail to complete the sequence. Point clouds are colorized by camera RGB. w/o sub. repla… view at source ↗
Figure 10
Figure 10. Figure 10: Runtime vs. peak memory on the 7 HILTI’22 sequences completed [PITH_FULL_IMAGE:figures/full_fig_p016_10.png] view at source ↗
Figure 11
Figure 11. Figure 11: Sensitivity of ATE to the SAIF gate threshold [PITH_FULL_IMAGE:figures/full_fig_p017_11.png] view at source ↗
Figure 12
Figure 12. Figure 12: LIO/VIO information complementarity on the [PITH_FULL_IMAGE:figures/full_fig_p018_12.png] view at source ↗
read the original abstract

Tightly coupled LiDAR-visual-inertial odometry (LIVO) fuses precise geometric depth with complementary visual measurements, yet its exteroceptive sensors face independent failure modes: LiDAR degenerates when scan geometry is under-constrained, while visual measurements degrade under adverse illumination or texture absence. Existing countermeasures, including binary degeneracy detection, covariance inflation, and scene-level quality gating, operate at the modality level and leave the direction-dependent structure of the joint information matrix unaddressed. Consequently, visual residuals enter pose directions where LiDAR is well-constrained, while in deficient directions visual compensation disperses across the full state space rather than concentrating where needed. We propose SA-LIVO, a LiDAR-inertial-visual odometry system addressing these limitations through direction-selective fusion and information-efficient processing. The Subspace-Aware Information Fusion (SAIF) framework eigendecomposes the joint LiDAR-visual information matrix and applies a linear-clamp soft gate per eigendirection, attenuating degenerate directions while preserving observable ones at full strength. LiDAR and visual residuals are then jointly optimized in one InEKF loop at a shared linearization point. Since visual information contributes only where LiDAR is deficient, photometric Jacobians are assembled once before the loop and reused across iterations, avoiding the per-iteration cost of conventional iterated filters. Experiments on 29 sequences from three benchmarks (HILTI'22, New College, Oxford Spires) and concurrent-degradation scenarios show accuracy competitive with the strongest baselines and bounded drift where competing systems diverge. SA-LIVO averages 12.3 ms per frame on a laptop CPU and 26.8 ms on an embedded ARM board without GPU, with 3.6-6.3x lower peak memory. The code will be open-sourced.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, simulated authors' rebuttal, and a circularity audit. Tearing a paper down is the easy half of reading it; the pith above is the substance, this is the friction.

Referee Report

2 major / 2 minor

Summary. The manuscript presents SA-LIVO, a tightly-coupled LiDAR-inertial-visual odometry system. It introduces the Subspace-Aware Information Fusion (SAIF) framework that eigendecomposes the joint LiDAR-visual information matrix and applies a linear-clamp soft gate per eigendirection to attenuate degenerate directions while preserving observable ones at full strength. LiDAR and visual residuals are jointly optimized in one InEKF loop at a shared linearization point, with photometric Jacobians precomputed once and reused across iterations. Experiments on 29 sequences from HILTI'22, New College, and Oxford Spires benchmarks, plus concurrent-degradation scenarios, report competitive accuracy, bounded drift where baselines diverge, 12.3 ms/frame on laptop CPU, 26.8 ms on embedded ARM, and 3.6-6.3x lower peak memory. The code will be open-sourced.

Significance. If the SAIF eigendecomposition and soft-gate mechanism correctly isolate and selectively attenuate degenerate subspaces without misalignment, the method provides a principled direction-dependent fusion approach that improves upon modality-level gating in existing LIVO systems. The single-loop InEKF optimization with Jacobian reuse yields clear efficiency gains. The empirical results on multiple benchmarks and hardware platforms, combined with the commitment to open-source the code, indicate practical value for real-time robotics in challenging environments where independent sensor failures occur.

major comments (2)
  1. [SAIF framework] SAIF framework (joint information matrix eigendecomposition and linear-clamp gate): the central claim that eigendirections at the shared linearization point cleanly separate observable and degenerate subspaces of the combined cost is load-bearing for selective attenuation. The manuscript does not address the risk that residual gradients may deviate from this linearization (due to nonlinearity, iteration drift, or sensor-specific geometry), which could cause the gate to over-damp observable directions or under-attenuate degenerate ones. A concrete analysis or additional validation of this alignment is required.
  2. [Experiments] Experiments (29 sequences and concurrent-degradation scenarios): while competitive accuracy and bounded drift are reported, the results do not isolate the contribution of the per-eigendirection soft gate from other components such as the shared InEKF linearization point or Jacobian reuse. Without such ablations or controls, it is difficult to attribute the robustness gains specifically to the subspace-aware handling.
minor comments (2)
  1. [Abstract] Abstract: the runtime and memory claims (12.3 ms/frame, 26.8 ms on ARM, 3.6-6.3x lower peak memory) are presented without explicit comparison to the runtimes or memory of the strongest baselines on the same hardware.
  2. The description of the linear-clamp soft gate would benefit from an explicit equation or pseudocode to clarify the clamping thresholds and how they interact with the eigendecomposition.

Simulated Author's Rebuttal

2 responses · 0 unresolved

We thank the referee for the constructive and detailed feedback. We respond to each major comment below and indicate where revisions will be made to address valid concerns.

read point-by-point responses

  1. Referee: [SAIF framework] SAIF framework (joint information matrix eigendecomposition and linear-clamp gate): the central claim that eigendirections at the shared linearization point cleanly separate observable and degenerate subspaces of the combined cost is load-bearing for selective attenuation. The manuscript does not address the risk that residual gradients may deviate from this linearization (due to nonlinearity, iteration drift, or sensor-specific geometry), which could cause the gate to over-damp observable directions or under-attenuate degenerate ones. A concrete analysis or additional validation of this alignment is required.

    Authors: We acknowledge that the manuscript does not explicitly analyze potential misalignment between eigendirections at the shared linearization point and residual gradients during optimization due to nonlinearity or iteration effects. The InEKF single-loop design and Jacobian reuse aim to preserve consistency at the common point, with the linear-clamp gate providing a soft mechanism for robustness. However, the referee's point on the need for concrete validation is valid. In the revised version we will add a dedicated subsection with a sensitivity study quantifying eigendirection drift across iterations on representative sequences and its effect on gate values. revision: yes

  2. Referee: [Experiments] Experiments (29 sequences and concurrent-degradation scenarios): while competitive accuracy and bounded drift are reported, the results do not isolate the contribution of the per-eigendirection soft gate from other components such as the shared InEKF linearization point or Jacobian reuse. Without such ablations or controls, it is difficult to attribute the robustness gains specifically to the subspace-aware handling.

    Authors: The referee correctly identifies that the current experiments do not include ablations that isolate the per-eigendirection soft gate from the shared linearization point and Jacobian reuse. The concurrent-degradation scenarios demonstrate overall system behavior under independent sensor failure, but they do not disentangle the subspace-aware component. We will incorporate additional ablation studies in the revision, comparing the full SAIF system against variants that disable the per-eigendirection gating while retaining the other optimizations, to better attribute the observed robustness gains. revision: yes

Circularity Check

0 steps flagged

No circularity: SAIF framework is a novel processing step with independent derivation

full rationale

The paper introduces the Subspace-Aware Information Fusion (SAIF) as a new eigendecomposition-based gating mechanism applied to the joint LiDAR-visual information matrix, followed by joint InEKF optimization. No equations or claims in the provided text reduce the claimed performance, degeneracy handling, or efficiency gains to a fitted parameter, self-citation chain, or input by construction. The method is presented as a direct algorithmic contribution without invoking prior author work as a uniqueness theorem or ansatz. Experiments are described as external validation on public benchmarks. This satisfies the default expectation of a self-contained derivation.

Axiom & Free-Parameter Ledger

0 free parameters · 1 axioms · 0 invented entities

Only the abstract is available; the ledger is therefore limited to the core modeling choice stated in the abstract.

axioms (1)
  • domain assumption The joint LiDAR-visual information matrix admits an eigendecomposition whose directions correspond to observable versus degenerate subspaces of the fused state.
    Invoked by the SAIF framework description in the abstract.

pith-pipeline@v0.9.1-grok · 5878 in / 1213 out tokens · 22608 ms · 2026-06-25T21:09:24.362413+00:00 · methodology

discussion (0)

Sign in with ORCID, Apple, or X to comment. Anyone can read and Pith papers without signing in.

Reference graph

Works this paper leans on

37 extracted references · 36 canonical work pages

  1. [1]

    FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry,

    C. Zheng, W. Xu, Q. Guo, and F. Zhang, “FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry,”IEEE Trans. Robot., vol. 40, pp. 1529– 1546, 2024, doi: 10.1109/TRO.2024.3502198

    work page doi:10.1109/tro.2024.3502198 2024

  2. [2]

    Generalized affordance templates for mobile manipulation,

    J. Lin and F. Zhang, “R 3LIVE: A robust, real-time, RGB- colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” inProc. IEEE Int. Conf. Robot. Autom. (ICRA), Philadelphia, PA, USA, May 2022, pp. 10672–10678, doi: 10.1109/ICRA46639.2022.9812253

    work page doi:10.1109/icra46639.2022.9812253 2022

  3. [3]

    LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping,

    T. Shan, B. Englot, C. Ratti, and D. Rus, “LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping,” inProc. IEEE Int. Conf. Robot. Autom. (ICRA), Xi’an, China, May 2021, pp. 5692–5698

    2021

  4. [4]

    The Oxford Spires Dataset: Benchmarking large-scale LiDAR-visual localisation, reconstruction and radiance field methods,

    Y . Taoet al., “The Oxford Spires Dataset: Benchmarking large-scale LiDAR-visual localisation, reconstruction and radiance field methods,” Int. J. Robot. Res., 2025, doi: 10.1177/02783649251369905

    work page doi:10.1177/02783649251369905 2025

  5. [5]

    Hilti-Oxford dataset: A millimetre accurate benchmark for simultaneous localization and mapping,

    L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-Oxford dataset: A millimetre accurate benchmark for simultaneous localization and mapping,”IEEE Robot. Autom. Lett., vol. 8, no. 1, pp. 408–415, Jan. 2023, doi: 10.1109/LRA.2022.3226077

    work page doi:10.1109/lra.2022.3226077 2023

  6. [6]

    The Newer College Dataset: Handheld LiDAR, inertial and vision with ground truth,

    M. Ramezani, Y . Wang, M. Camurri, D. Wisth, M. Mattamala, and M. Fallon, “The Newer College Dataset: Handheld LiDAR, inertial and vision with ground truth,” inProc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Las Vegas, NV , USA, Oct. 2020, pp. 4353–4360, doi: 10.1109/IROS45743.2020.9340849

    work page doi:10.1109/iros45743.2020.9340849 2020

  7. [7]

    LOAM: Lidar odometry and mapping in real- time,

    J. Zhang and S. Singh, “LOAM: Lidar odometry and mapping in real- time,” inProc. Robot.: Sci. Syst. (RSS), Berkeley, CA, USA, Jul. 2014, doi: 10.15607/RSS.2014.X.007

    work page doi:10.15607/rss.2014.x.007 2014

  8. [8]

    IEEE Transactions on Robotics38(4), 2053–2073 (2022) https: //doi.org/10.1109/TRO.2022.3141876

    W. Xu, Y . Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,”IEEE Trans. Robot., vol. 38, no. 4, pp. 2053– 2073, Aug. 2022, doi: 10.1109/TRO.2022.3141876

    work page doi:10.1109/tro.2022.3141876 2053

  9. [9]

    Laser–visual–inertial odometry and mapping with high robustness and low drift,

    J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping with high robustness and low drift,”J. Field Robot., vol. 35, no. 8, pp. 1242–1264, 2018, doi: 10.1002/rob.21809

    work page doi:10.1002/rob.21809 2018

  10. [10]

    In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

    A. Hinduja, B.-J. Ho, and M. Kaess, “Degeneracy-aware factors with applications to underwater SLAM,” inProc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Macau, China, Nov. 2019, pp. 1293–1299, doi: 10.1109/IROS40897.2019.8968577

    work page doi:10.1109/iros40897.2019.8968577 2019

  11. [11]

    X-ICP: Localizability-aware LiDAR registration for robust localization in ex- treme environments,

    T. Tuna, J. Nubert, Y . Nava, S. Khattak, and M. Hutter, “X-ICP: Localizability-aware LiDAR registration for robust localization in ex- treme environments,”IEEE Trans. Robot., vol. 40, pp. 452–471, 2024, doi: 10.1109/TRO.2023.3335691

    work page doi:10.1109/tro.2023.3335691 2024

  12. [12]

    Efficient and prob- abilistic adaptive voxel mapping for accurate online LiDAR odometry,

    C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and prob- abilistic adaptive voxel mapping for accurate online LiDAR odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 8518–8525, 2022, doi: 10.1109/LRA.2022.3185439

    work page doi:10.1109/lra.2022.3185439 2022

  13. [13]

    KISS-ICP: In defense of point-to-point ICP – sim- ple, accurate, and robust registration if done the right way,

    I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, “KISS-ICP: In defense of point-to-point ICP – sim- ple, accurate, and robust registration if done the right way,”IEEE Robot. Autom. Lett., vol. 8, no. 2, pp. 1029–1036, Feb. 2023, doi: 10.1109/LRA.2023.3236571

    work page doi:10.1109/lra.2023.3236571 2023

  14. [14]

    A multi-state constraint Kalman filter for vision-aided inertial navigation,

    A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” inProc. IEEE Int. Conf. Robot. Autom. (ICRA), Roma, Italy, Apr. 2007, pp. 3565–3572, doi: 10.1109/ROBOT.2007.364024

    work page doi:10.1109/robot.2007.364024 2007

  15. [15]

    Hierarchical coverage path planning in com- plex 3d environments,

    P. Geneva, K. Eckenhoff, W. Lee, Y . Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” inProc. IEEE Int. Conf. Robot. Autom. (ICRA), Paris, France, May 2020, pp. 4666–4672, doi: 10.1109/ICRA40945.2020.9196524

    work page doi:10.1109/icra40945.2020.9196524 2020

  16. [16]

    Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback,

    M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback,”Int. J. Robot. Res., vol. 36, no. 10, pp. 1053– 1072, 2017, doi: 10.1177/0278364917728574

    work page doi:10.1177/0278364917728574 2017

  17. [17]

    Vins-mono: A robust and versatile monocular visual-inertial state estimator

    T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,”IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018, doi: 10.1109/TRO.2018.2853729

    work page doi:10.1109/tro.2018.2853729 2018

  18. [18]

    doi:10.1109/TRO.2021.3075643 , file =

    C. Campos, R. Elvira, J. J. G. Rodr ´ıguez, J. M. M. Montiel, and J. D. Tard´os, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,”IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021, doi: 10.1109/TRO.2021.3075644

    work page doi:10.1109/tro.2021.3075644 2021

  19. [19]

    and Lynen, S

    S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimiza- tion,”Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, 2015, doi: 10.1177/0278364914554813

    work page doi:10.1177/0278364914554813 2015

  20. [20]

    On- manifold preintegration for real-time visual–inertial odometry,

    C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On- manifold preintegration for real-time visual–inertial odometry,” IEEE Trans. Robot., vol. 33, no. 1, pp. 1–21, Feb. 2017, doi: 10.1109/TRO.2016.2597321

    work page doi:10.1109/tro.2016.2597321 2017

  21. [21]

    Direct sparse odometry,

    J. Engel, V . Koltun, and D. Cremers, “Direct sparse odometry,”IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, 2018, doi: 10.1109/TPAMI.2017.2658577

    work page doi:10.1109/tpami.2017.2658577 2018

  22. [22]

    SVO: Fast semi- direct monocular visual odometry,

    C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi- direct monocular visual odometry,” inProc. IEEE Int. Conf. Robot. Autom. (ICRA), Hong Kong, China, May 2014, pp. 15–22, doi: 10.1109/ICRA.2014.6906584

    work page doi:10.1109/icra.2014.6906584 2014

  23. [23]

    LSD-SLAM: Large-scale direct monocular SLAM,

    J. Engel, T. Sch ¨ops, and D. Cremers, “LSD-SLAM: Large-scale direct monocular SLAM,” inProc. Eur. Conf. Comput. Vis. (ECCV), Z ¨urich, Switzerland, Sep. 2014, pp. 834–849, doi: 10.1007/978-3-319-10605- 2 54

    work page doi:10.1007/978-3-319-10605- 2014

  24. [24]

    Observability- based rules for designing consistent EKF SLAM estimators,

    G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Observability- based rules for designing consistent EKF SLAM estimators,” Int. J. Robot. Res., vol. 29, no. 5, pp. 502–528, 2010, doi: 10.1177/0278364909353640

    work page doi:10.1177/0278364909353640 2010

  25. [25]

    Consistency analysis and improvement of vision-aided inertial navi- gation,

    J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Consistency analysis and improvement of vision-aided inertial navi- gation,”IEEE Trans. Robot., vol. 30, no. 1, pp. 158–176, Feb. 2014, doi: 10.1109/TRO.2013.2277549

    work page doi:10.1109/tro.2013.2277549 2014

  26. [26]

    The invariant extended Kalman filter as a stable observer,

    A. Barrau and S. Bonnabel, “The invariant extended Kalman filter as a stable observer,”IEEE Trans. Autom. Control, vol. 62, no. 4, pp. 1797– 1812, Apr. 2017, doi: 10.1109/TAC.2016.2594085

    work page doi:10.1109/tac.2016.2594085 2017

  27. [27]

    Exploiting symmetries to design EKFs with consistency properties for navigation and SLAM,

    M. Brossard, A. Barrau, and S. Bonnabel, “Exploiting symmetries to design EKFs with consistency properties for navigation and SLAM,” IEEE Sensors J., vol. 19, no. 4, pp. 1572–1579, Feb. 2019, doi: 10.1109/JSEN.2018.2882714

    work page doi:10.1109/jsen.2018.2882714 2019

  28. [28]

    Hartley, M

    R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact- aided invariant extended Kalman filtering for robot state estima- tion,”Int. J. Robot. Res., vol. 39, no. 4, pp. 402–430, 2020, doi: 10.1177/0278364919894385

    work page doi:10.1177/0278364919894385 2020

  29. [29]

    Invariant ex- tended Kalman filtering for tightly coupled LiDAR-inertial odom- IEEE TRANSACTIONS ON ROBOTICS 20 etry and mapping,

    P. Shi, Z. Zhu, S. Sun, X. Zhao, and M. Tan, “Invariant ex- tended Kalman filtering for tightly coupled LiDAR-inertial odom- IEEE TRANSACTIONS ON ROBOTICS 20 etry and mapping,”IEEE/ASME Trans. Mechatron., 2023, doi: 10.1109/TMECH.2022.3233363

    work page doi:10.1109/tmech.2022.3233363 2023

  30. [30]

    A high-precision LiDAR- inertial odometry via invariant extended Kalman filtering and efficient surfel mapping,

    H. Zhang, R. Xiao, J. Li, C. Yan, and H. Tang, “A high-precision LiDAR- inertial odometry via invariant extended Kalman filtering and efficient surfel mapping,”IEEE Trans. Instrum. Meas., vol. 73, pp. 1–11, 2024, Art no. 8502911, doi: 10.1109/TIM.2024.3382751

    work page doi:10.1109/tim.2024.3382751 2024

  31. [31]

    LIMO: Lidar-monocular visual odometry,

    J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: Lidar-monocular visual odometry,” inProc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Madrid, Spain, Oct. 2018, pp. 7872–7879, doi: 10.1109/IROS.2018.8594394

    work page doi:10.1109/iros.2018.8594394 2018

  32. [32]

    In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

    X. Zuo, P. Geneva, W. Lee, Y . Liu, and G. Huang, “LIC-Fusion: LiDAR-inertial-camera odometry,” inProc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Macau, China, Nov. 2019, pp. 5848–5854, doi: 10.1109/IROS40897.2019.8967746

    work page doi:10.1109/iros40897.2019.8967746 2019

  33. [33]

    R 2LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping,

    J. Lin, C. Zheng, W. Xu, and F. Zhang, “R 2LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robot. Autom. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021, doi: 10.1109/LRA.2021.3095515

    work page doi:10.1109/lra.2021.3095515 2021

  34. [34]

    In 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

    C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry,” inProc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Kyoto, Japan, Oct. 2022, pp. 4003–4009, doi: 10.1109/IROS47612.2022.9981107

    work page doi:10.1109/iros47612.2022.9981107 2022

  35. [35]

    Hierarchical coverage path planning in com- plex 3d environments,

    A. Rosinol, M. Abate, Y . Chang, and L. Carlone, “Kimera: An open- source library for real-time metric-semantic localization and mapping,” inProc. IEEE Int. Conf. Robot. Autom. (ICRA), Paris, France, May 2020, pp. 1689–1696, doi: 10.1109/ICRA40945.2020.9196885

    work page doi:10.1109/icra40945.2020.9196885 2020

  36. [36]

    VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,

    D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,”IEEE Trans. Robot., vol. 39, no. 1, pp. 309–326, Feb. 2023, doi: 10.1109/TRO.2022.3193788

    work page doi:10.1109/tro.2022.3193788 2023

  37. [37]

    SR-LIVO: LiDAR-inertial-visual odometry and mapping with sweep reconstruc- tion,

    Z. Yuan, J. Deng, R. Ming, F. Lang, and X. Yang, “SR-LIVO: LiDAR-inertial-visual odometry and mapping with sweep reconstruc- tion,”IEEE Robot. Autom. Lett., vol. 9, no. 6, pp. 5110–5117, 2024, doi: 10.1109/LRA.2024.3385654

    work page doi:10.1109/lra.2024.3385654 2024