Visual-Inertial SLAM

• VI-SLAM is a real-time method that fuses visual and inertial measurements to achieve robust localization and mapping.
• It employs tightly-coupled optimization frameworks with vision, IMU, and GPS data to maintain centimeter-level accuracy over long trajectories.
• Recent advances include uncertainty-aware updates, loop closure integration, and hardware-software codesign to tackle dynamic and unstructured environments.

Visual-Inertial Simultaneous Localization and Mapping (VI-SLAM) is a field-defining paradigm for real-time, drift-robust state estimation and global mapping in robotics, AR/VR, autonomous navigation, and beyond. At its core, VI-SLAM tightly fuses visual measurements—stereo or monocular image frames—with high-rate inertial data, leveraging complementary strengths of both: vision yields rich geometric information, while inertial sensors (IMU) capture short-term motion dynamics and resolve scale ambiguities. The state-of-the-art extends this basic formulation with loop closure, global sensor fusion (e.g. GPS), uncertainty-aware mapping, dynamic environment robustness, hardware-software codesign, and active SLAM strategies. Tightly-coupled optimization frameworks serve as the algorithmic backbone, yielding centimeter-level consistency over long-term, large-scale, and highly unstructured trajectories.

1. Architectural Foundations and State Modeling

VI-SLAM systems jointly estimate trajectory, velocity, and biases of the moving agent in a sliding window of keyframes. The canonical state vector is

$$x = \left[\ T_{WS}^i,\ v_{W}^i,\ b_g^i,\ b_a^i\ \right]_{i=1..K}$$

where $T_{WS}^i \in SE(3)$ is the IMU pose for keyframe $i$, $v_W^i$ is velocity, and $b_g^i$, $b_a^i$ are gyroscope and accelerometer biases respectively. Feature positions $P_l \in \mathbb{R}^3$ may be included or marginalized in optimization. Sensor modalities consist of synchronized stereo (or monocular) cameras for feature detection, description, and triangulation, high-rate IMU for motion propagation and preintegration, and—when available—global sources like GPS for drift-free anchoring (Boche et al., 2022).

IMU readings are preintegrated between keyframes, yielding predicted pose and velocity increments via closed-form accumulations of rotated accelerations and angular velocities (e.g., Forster et al. preintegration). Stereo reprojection errors directly penalize the deviation of observed landmarks from projected estimates. All measurement uncertainties propagate through the optimization via information matrices—typically inverse covariance.

2. Factor-Graph Optimization and Measurement Integration

VI-SLAM implements a nonlinear least-squares optimization over a factor graph incorporating multiple types of residuals:

• Vision (reprojection) residuals: For each visual observation, the error $r_{vis} = z_{il} - p_{pred}$ reflects deviation between measured image features and their predicted projections given the estimated pose and map. Jacobians are computed via chain rule over camera intrinsics and pose variables.
• IMU preintegration residuals: $r_{imu}(x_i, x_{i+1})$ encodes the difference between preintegrated inertial increments and predicted state transitions, capturing local motion and aiding short-term prediction during visual dropout.
• GPS (or other global) residuals: $r_{gps}$ ties selected preintegrated poses to low-rate global fixes, providing absolute position anchoring.
• Loop closure constraints: When re-visiting areas, visual place recognition coupled with geometric verification introduces loop-closure factors, correcting drift accumulated over long trajectories.

These are combined in a global cost function:

$$J(x,T_{GW}) = \sum_{(i,l)\in\Omega_{vis}} r_{vis}(x_i, P_l)^T W_v r_{vis}(x_i, P_l) + \sum_{i=1}^{K-1} r_{imu}(x_i,x_{i+1})^T W_i r_{imu}(x_i,x_{i+1}) + \sum_{j\in G} r_{gps}(x_i,T_{GW})^T W_g^j r_{gps}(x_i,T_{GW})$$

where $W_v$, $W_i$, $W_g^j$ are information matrices, and $T_{GW}$ aligns the local frame to the global frame (Boche et al., 2022).

3. Initialization, Global Alignment, and Drift Correction

Fusion of GPS or other global sources necessitates robust initialization and drift management. The system collects VIO pose and GPS correspondences and solves an SVD-based alignment (Horn's method) for 4-DOF extrinsic transformation (yaw and translation), then monitors yaw observability via the Hessian's variance ($P_{\theta\theta} < \sigma_\theta^2$). Upon GPS dropout, a two-stage correction is performed:

1. Immediate Position Alignment: On GPS recovery, the position drift $\Delta r$ between the current VIO state and new GPS fix is distributed uniformly over the states since last GPS reception, re-aligning the trajectory.
2. Full Yaw+Position Realignment: When multiple fresh fixes are obtained, a new $T_{GW}$ is computed, and delta transform $\Delta T$ (yaw and translation) is applied to affected states. Chordal rotation averaging ensures orientation correction is robust. Backend optimization then refines all factors (vision, inertial, GPS, loop-closure).

This approach remedies accumulated drift during extended GPS outages and integrates corrections with the local VIO window, preserving scalability for long sequences (Boche et al., 2022).

4. Robustness in Dynamic and Unstructured Environments

Modern VI-SLAM approaches address dynamic scene challenges and unstructured environments through a combination of motion prior modeling, geometric elimination, and adaptive residual weighting:

• Motion Prior SLAM: In dynamic environments, inertial preintegration yields a geometric motion prior for each feature track, and the minimum epipolar projection error ($d_s$) is computed, reflecting consistency with the IMU-propagated motion (Sun et al., 30 Mar 2025). During feature tracking, tracks with largest $d_s$ are eliminated, remaining dynamic candidates are adaptively down-weighted in bundle adjustment using Huber kernels and prior-based covariances, yielding robust localization and clean static maps.
• Uncertainty Propagation in Dense Mapping: Dense submaps with volumetric occupancy can be fused with visual-inertial front-ends, where aleatoric depth uncertainty from deep networks is propagated into both occupancy probabilities and alignment factors between submaps. Bayesian depth fusion across stereo and temporal baselines increases mapping accuracy and robustness, with uncertainty-aware updates ensuring real-time planning compatibility and reliable geometry (Jung et al., 18 Sep 2024).
• Volumetric Submaps and Trajectory Anchoring: For MAV deployment in dense forests, submaps are anchored to active VI-SLAM keyframes, and reference trajectories are deformed through proximity-based and quaternion averaging schemes whenever state estimates change, ensuring continuity and safety in path planning despite loop-closure corrections or drift (Laina et al., 14 Mar 2024).

5. Hardware-Software Codesign and Sensor Fusion Extensions

Efficient VI-SLAM demands tight hardware-software integration:

• Integrated sensor fusion hardware: On-board platforms like PIRVS synchronize global-shutter stereo cameras and IMU at interface latency $<10$ ns, with deterministic timestamping and calibration yielding sub-pixel intrinsic/extrinsic errors. EKF filtering forms the tightly-coupled VIO backbone, with parallel mapping and local bundle adjustment in dedicated threads. Loosely-coupled additional modalities (e.g., GNSS) may be included via centralized EKF updates (Zhang et al., 2017).
• Multi-camera and novel sensors: Systems extend to multi-camera arrays, wheel odometry, and magnetometers. Dense free-space mapping is achievable through pixel-wise segmentation fused with homography-based 3D lifting, enabling autonomous valet parking and large-scale operation with sub-1% drift (Abate et al., 2023). Underwater environments integrate acoustic Doppler velocity logs (DVL) or sonar for scale, with efficient online extrinsic and beam misalignment calibration, ensuring robust operation in low-visibility, visually-degraded regimes (Xu et al., 14 Mar 2025, Zhang et al., 18 Jun 2025).

6. Experimental Performance and Benchmarking

Robustness and accuracy of VI-SLAM frameworks are established in challenging public benchmarks and real-world deployments. Quantitative results include:

• EuRoC MAV: Median ATE reduced from $\sim0.064$ m (OKVIS2 VIO) to $0.025$ m with tightly-coupled GPS; matches or excels beyond VINS-Fusion, other TC-VIO (Boche et al., 2022).
• Forest UAV trials: Dense occupancy submapping, trajectory anchoring, and VI-SLAM integration yield $<0.5$\% drift over 226 m, with zero collisions at flight speeds up to 4 m/s (Laina et al., 14 Mar 2024).
• Dynamic urban scenes: Adaptive motion-prior filtering results in ATE RMSE up to 40% lower in high dynamicity than prior art, with negligible runtime overhead (Sun et al., 30 Mar 2025).
• Dense Volumetric Mapping: Uncertainty-aware fusion improves localization and mapping accuracy, sub-3 cm ATE and $>60\%$ completeness in multi-camera and stereo setups (Jung et al., 18 Sep 2024).
• Underwater localization: Gravity-enhanced stereo VI-SLAM and acoustic-visual-inertial frameworks outperform state-of-the-art with ATE $<0.012$ m in rich, degenerate, and dynamic environments (Shen et al., 28 Oct 2025, Xu et al., 14 Mar 2025).

7. Open Challenges, Limitations, and Future Directions

Key limitations persist:

• Accurate GPS-antenna lever-arm and time offset calibration remain open issues; online methods are needed for scalable, real-world integration (Boche et al., 2022).
• Multipath and urban canyon degrade GPS covariance estimates, motivating fusion with additional global cues (magnetometer, barometer, pseudo-ranges).
• Computational scaling for tens-of-kilometers trajectories will require hierarchical graph summarization and efficient graph pruning.
• Dynamic environment handling could be further enhanced by multi-rigid-body segmentation and semantic integration; non-rigid dynamic modeling is still largely unexplored.
• Robustness to severe sensor degradation, long-term autonomy, and adaptation to novel sensor platforms (LiDAR, event cameras, panoramic encoders) present active research directions.

Recent advances—data-driven depth uncertainty fusion, bias-eliminated estimation, reinforcement learning for active SLAM—define next-generation capabilities. Continued benchmarking and open-source evaluation on datasets targeting real-world complexity, e.g., InCrowd-VI for crowded human navigation (Bamdad et al., 21 Nov 2024), drive rigorous progress toward universally robust VI-SLAM.