--- title: 'FAST-LIVO2: Efficient LiDAR-Inertial Visual Odometry' url: https://www.emergentmind.com/topics/fast-livo2 type: topic --- # FAST-LIVO2: Efficient LiDAR-Inertial Visual Odometry FAST-LIVO2 refers to a class of efficient, direct LiDAR-Inertial-Visual Odometry (LIVO) systems designed for accurate, real-time state estimation and mapping in robotics and autonomous systems. It extends tightly-coupled sensor fusion with algorithmic advances in sequential Kalman filtering, direct registration, unified mapping, and computational efficiency. The framework is widely recognized for its state-of-the-art accuracy, robustness to sensor degeneracy, and suitability for resource-constrained embedded platforms [2408.14035, 2501.13876]. ## 1. System Model and Sensor Fusion Pipeline FAST-LIVO2 implements a tightly-coupled fusion of LiDAR, IMU, and monocular (or stereo) vision streams within a sliding-window recursive estimator. The system architecture includes: - **Scan Recombination and Motion Compensation**: Raw, high-frequency LiDAR points are synchronized and motion-compensated to the camera timestamp using inertial mechanization. - **Error-State Iterated Kalman Filter (ESIKF)**: The core estimator integrates IMU, LiDAR, and image measurements on the manifold, with a 19-dimensional state comprising attitude, position, velocity, biases, gravity, and photometric exposure [2408.14035, 2501.13876]. - **Sequential Update Strategy**: Modality-dependent dimension mismatch is managed by factorizing the posterior. LiDAR and visual updates are carried out sequentially within the IEKF: LiDAR measurements update first for geometric alignment, followed by visual measurements for photometric consistency [2408.14035]. - **Unified Voxel Map**: All geometric (LiDAR) and visual (image patch) information is registered in a single, sliding-window, hash-indexed voxel octree structure, enabling direct fusion at the map level [2408.14035]. The overall pipeline at each processing cycle is as follows: IMU propagation → LiDAR ESIKF update → Visual ESIKF update → Local map geometry/patch update. ## 2. Key Algorithmic Components ### Direct Registration Both LiDAR and visual modules use direct registration: - **LiDAR Point-to-Plane**: For each de-skewed LiDAR point, a local plane is fitted from neighboring map points. The system minimizes the point-to-plane residual—without explicit edge/plane feature extraction—within the IEKF [2107.06829, 2408.14035]. - **Sparse-Direct Photometric Alignment**: For selected visual map points, direct pixel-level photometric error is minimized using affine-warped image patches, accounting for exposure time as part of the state [2408.14035]. ### Unified Mapping - **Voxel Structure**: Root voxels (0.5 m³) are managed in a hash map; each leaf holds plane priors along with visual map points with patch pyramids. This structure provides mutual benefit: LiDAR-derived plane priors inform photometric tracking; image exposure/lighting is adaptively estimated and updated [2408.14035]. - **Reference Patch Selection and Raycasting**: Reference patches are dynamically scored and replaced for robustness. On-demand raycasting is used in visual blind zones, extracting candidate features via voxel lookups based on image grid coverage [2408.14035]. ## 3. Performance Metrics and Comparative Benchmarks FAST-LIVO2 demonstrates significant advances in multiple performance dimensions: | Method | Translational RMSE (m) | CPU (ms/frame, x86) | Embedded (ms/frame, ARM) | Memory (GB) | Dataset | |----------------|-----------------------|---------------------|--------------------------|-------------|----------------| | FAST-LIVO2 | 0.044 | 30 | 78 | 2.5 | Hilti, Mars-LVIG| | FAST-LIO2 | 0.151 | — | — | — | — | | LVI-SAM | 1.93 | — | — | — | — | | Ours† [2501...]| 0.063 | 26 | 57.8 | 1.7 | Hilti, Private | †"Ours" denotes a resource-optimized variant of FAST-LIVO2 with adaptive visual keyframing and a two-tiered map [2501.13876]. Key insights: - Robust centimeter-level accuracy (RMSE as low as 0.044 m), outperforming feature-based and indirect fusion competitors in both public and private datasets. - Real-time operation at 10 Hz on commodity x86 CPUs (<31 ms/frame), and on embedded ARM boards (<80 ms/frame). - Robustness under sensor degeneracy and severe brightness or texture changes (via on-demand raycasting and dynamic exposure estimation). - Memory usage and computational footprint are further reduced in variants for edge devices, with only minor accuracy trade-offs [2501.13876]. ## 4. Handling Sensor Degeneracy and Resource Constraints FAST-LIVO2 and its derivatives implement principled strategies for managing sensor degeneracy: - **Degeneracy-Aware Visual Frame Selector**: Employs a LiDAR-constraint metric based on the normal matrix spectrum. In degenerate scenes (e.g., featureless walls), all images are promoted to keyframes, ensuring visual tracking is not compromised [2501.13876]. - **Compute and Memory Efficiency**: A hybrid mapping strategy is used: a small, local robocentric surfel map (LiDAR + visual patches) is complemented by a long-term global visual map for sparse features, culling out-of-range voxels and detaching surfels when possible. This achieves a ≈47% memory reduction and ≈33% runtime reduction with only a ≈3 cm RMSE increase [2501.13876]. - **ARM-Specific Optimization**: Evaluation on ARM SoCs demonstrates robust operation (<2 cm drift in real-world trials) and real-time performance in embedded deployments [2501.13876]. ## 5. Applications and Downstream Uses FAST-LIVO2 supports a range of robotics and vision applications: - **Onboard UAV Navigation**: Onboard compute enables closed-loop control with end-to-end drifts <10 cm in indoor, outdoor, and narrow-opening tests [2408.14035]. - **Airborne Mapping and City-Scale Modeling**: Demonstrated in Mars-LVIG benchmarks with dense, colored point clouds produced in real time and RMSE down to 0.27 m [2408.14035]. - **3D Reconstruction and Rendering**: Dense maps are suitable for mesh+texture generation and data-driven NeRF/3DGS pipelines, offering high-quality geometry and photometric consistency [2408.14035]. - **Resource-Constrained Deployment**: The system runs efficiently on edge devices, enabling real-time LIVO on platforms without a discrete GPU [2501.13876]. ## 6. Algorithmic Innovations and Comparison to Prior Work FAST-LIVO2 builds on the foundational FAST-LIO2 LiDAR-inertial system [2107.06829] by introducing: - **Direct LiDAR-Visual Fusion**: Prior works often required explicit, hand-engineered extraction of edge, plane, or point features, or relied on loosely-coupled optimization. FAST-LIVO2 eliminates this, directly fusing raw sensor streams for improved geometric and photometric observability [2408.14035]. - **Sequential ESIKF**: Sequential handling of multimodal measurements provides better numerical conditioning and compatibility with highly heterogeneous data [2408.14035]. - **Unified Map Supporting Direct Visual and Geometric Updates**: Plane priors and patch-pyramid structures enable cross-modal benefits and improved accuracy under ambiguous conditions [2408.14035]. - **Algorithmic Efficiency**: The incremental k-d tree ("ikd-Tree") of FAST-LIO2 was extended to hash-octree voxel management and hybrid feature mapping, reducing per-point insertion and query times to O(log N) [2107.06829]. - **Dynamic Reference Patching and Raycasting**: These methods robustify alignment in degenerate scenes, corresponding to observed robustness on long, featureless traversals [2408.14035]. ## 7. Open Source Availability and Extensibility The FAST-LIVO2 codebase, benchmarks, and datasets are openly available at https://github.com/hku-mars/FAST-LIVO2 and are fully ROS-compatible for rapid integration with robotic autonomy stacks [2408.14035]. Key parameters—patch size, map voxelization, LiDAR downsampling, and IMU noise—are configurable for deployment across a wide range of hardware and operational environments. --- References: - "FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry" [2408.14035] - "FAST-LIVO2 on Resource-Constrained Platforms: LiDAR-Inertial-Visual Odometry with Efficient Memory and Computation" [2501.13876] - "FAST-LIO2: Fast Direct LiDAR-inertial Odometry" [2107.06829] Source: https://www.emergentmind.com/topics/fast-livo2