An Efficient Closed-Form Solution to Full Visual-Inertial State Initialization

1. Problem Statement

Visual-Inertial Simultaneous Localization and Mapping (VI-SLAM) systems require a robust initialization phase to estimate critical states before the main tracking loop begins. These states include:

• Initial velocity ($v$)
• Gravity direction ($g$)
• Gyroscope bias ($b_g$)
• Accelerometer bias ($b_a$)

Current Limitations:

• Iterative Optimization: Most state-of-the-art methods (e.g., VINS-Mono, ORB-SLAM3) rely on nonlinear optimization (e.g., Levenberg-Marquardt) to jointly estimate these states. While accurate, these methods are computationally expensive, require careful seeding to avoid local minima, and often need long time windows (typically ~2 seconds) to gather sufficient observability.
• Partial Closed-Form Solutions: Previous analytical methods (e.g., Martinelli) often assume known sensor biases or fail to estimate the full state (specifically missing accelerometer bias or requiring iterative refinement).
• Latency vs. Accuracy Trade-off: There is a lack of methods that provide a fully analytical, closed-form solution for the entire visual-inertial state (including both biases) that is both fast and accurate enough for real-time startup.

2. Methodology

The authors propose a novel closed-form initialization method that avoids nonlinear optimization entirely. The approach relies on two core approximations valid for typical sensor hardware and motion patterns: small-rotation and constant-velocity.

A. Rotation and Gyroscope Bias Initialization

The method first estimates the relative rotation ($R_{ij}$) between two frames and the gyroscope bias ($b_g$).

• Small-Rotation Approximation: The authors assume the rotation vector $\theta$ is small ($\theta \to 0$). Under this assumption, rotation matrices commute up to second-order terms, and the exponential map of a sum approximates the product of exponentials.
• Analytical Derivation: By applying this approximation to the IMU pre-integration equations, the authors derive a linear relationship that allows $b_g$ to be solved analytically.
• Three Variants: They propose three specific closed-form solvers for $b_g$:
  1. Commutative Approximation: Directly solves using the product of exponentials.
  2. Average Approximation: Assumes constant rotation angles within the integration window.
  3. Arithmetic Average Approximation: Uses the arithmetic mean of gyroscope readings.
• Strategy: These are computed using only the first two frames of the sequence, as rotation estimation is robust even with short baselines, avoiding the drift accumulation seen in longer windows.

B. Full State Initialization (Velocity, Gravity, Accelerometer Bias)

Once $R_{ij}$ and $b_g$ are known, the remaining states ($v, g, b_a$) and feature depths are estimated.

• Linear Constraints: The relationship between inertial measurements and visual feature tracks is formulated as a linear system ($\Xi x = s$).
• Nuisance Variables: Feature depths ($\lambda$) are treated as nuisance variables. Instead of estimating them recursively, they are marginalized out or eliminated via the linear system structure, avoiding the need for a full Structure-from-Motion (SfM) pipeline during initialization.
• Solution: The system is solved via Singular Value Decomposition (SVD) to find the least-squares solution for the state vector $x = [g, v, b_a, \lambda]$.

C. Two-Stage Observability-Driven Triggering

To balance accuracy and latency, the authors introduce a two-stage criterion to determine when to trigger the full initialization:

1. Stage 1 (Translation Observability): Checks if the parallax between the first frame and the current frame exceeds a threshold. This ensures sufficient translational motion to decouple gravity from acceleration.
2. Stage 2 (Full Observability): Once Stage 1 is met, the system checks the numerical conditioning of the reduced Hessian matrix (after marginalizing depths). It monitors the ratio of singular values ($\sigma_{max}/\sigma_{min}$) to ensure the system is well-conditioned before solving. This prevents premature solving when the accelerometer bias is unobservable (e.g., during pure rotation or low excitation).

3. Key Contributions

1. First Full-State Closed-Form Solution: The paper presents the first analytical method that recovers the complete visual-inertial state, including both gyroscope and accelerometer biases, without any iterative optimization.
2. Small-Rotation Approximation: It successfully exploits the small-rotation assumption to decouple and linearize the IMU integration equations, yielding compact and numerically stable solutions.
3. Observability-Driven Latency Reduction: The proposed two-stage triggering scheme allows the system to initialize as soon as data is sufficient, significantly reducing the initialization window compared to fixed-time approaches.
4. Elimination of SfM Dependency: Unlike many baselines, the method does not require a full 3D structure reconstruction (SfM) during the initialization phase, drastically reducing computational overhead.

4. Experimental Results

Experiments were conducted on the EuRoC MAV dataset, comparing the proposed method against baselines like VINS-Mono, DRT-tight, and AS-MLE (ORB-SLAM3 initialization).

• Accuracy:
  • Achieved 10–20% lower initialization error compared to optimization-based baselines.
  • Velocity RMSE: 0.09 m/s (vs. 0.11–0.17 m/s for baselines).
  • Gravity Direction Error: 0.90° (vs. 1.03–2.66° for baselines).
  • Bias Estimation: Consistently achieved the lowest gyroscope and accelerometer bias errors across all sequences.
• Efficiency:
  • Computation Time: Total initialization time is 0.96 ms, which is 5× faster than DRT-tight, 32× faster than VINS-Mono, and 32× faster than AS-MLE.
  • Gyroscope Bias: The closed-form solver is ~194× faster than nonlinear optimization for bias estimation alone.
• Latency:
  • The adaptive two-stage criterion reduced the average initialization window from ~2.16 seconds (baselines) to 0.48 seconds (4× reduction).
• Robustness:
  • The small-rotation approximation remains highly accurate for accumulated rotations up to 1.5–2.0 degrees. Since 85% of consecutive frame pairs in EuRoC have rotations below this threshold, the method is valid for the vast majority of real-world scenarios.

5. Significance

This work addresses a critical bottleneck in VI-SLAM: the startup phase. By replacing heavy iterative optimization with a lightweight, analytical solution, the authors enable:

• Faster System Startup: Critical for applications requiring rapid deployment or frequent resets (e.g., drones, mobile robots).
• Resource-Constrained Deployment: The method is suitable for embedded systems with limited CPU power where iterative solvers are too slow or memory-intensive.
• Improved Robustness: By avoiding nonlinear optimization, the method eliminates the risk of converging to local minima due to poor seeding, providing a more reliable "first guess" for subsequent tracking loops.

In summary, the paper demonstrates that with careful mathematical approximations and observability-aware triggering, it is possible to achieve higher accuracy with significantly lower computational cost than current state-of-the-art optimization-based initialization methods.