Visual-inertial odometry

"The camera tells me where I am but not how fast; the gyro tells me how fast but forgets where. I let them argue sixty times a second."

A Self-Localizing AI Agent

Why camera and IMU are the canonical pairing

The what is a division of labor. An IMU reports specific force and angular rate at hundreds of hertz; integrating it (Chapter 24) gives orientation, velocity, and position, but the double integration of accelerometer bias makes position error grow cubically in time, so raw inertial dead reckoning is useless beyond a few seconds. A camera, by contrast, measures the bearing to scene points with essentially zero temporal drift, yet a single monocular camera cannot recover metric scale (a small nearby scene and a large distant one project identically) and it stalls under motion blur, low texture, or darkness. The why of pairing them is that each covers the other's blind spot: the camera anchors the IMU's drift, and the IMU supplies the metric scale, the gravity direction, and the high-rate motion that carries the estimate across the visual dropouts. This is the complementarity dividend of Chapter 48 in its most-deployed form.

Three properties fall out of adding inertial data that pure visual odometry cannot deliver, and they are the reason VIO, not visual-only odometry, ships in phones, headsets, and drones. First, metric scale: the accelerometer feels real acceleration in \(\text{m/s}^2\), which fixes the scale ambiguity of a monocular rig. Second, gravity alignment: at rest the accelerometer reads the gravity vector, pinning roll and pitch to an absolute reference so they never drift. Third, robustness through gaps: when the camera is blinded for 100 ms by a light or a fast turn, inertial integration coasts through with bounded error.

Tight coupling and IMU preintegration

The how divides on one decision: loose versus tight coupling. A loosely coupled system runs visual odometry and inertial integration as separate black boxes and fuses their pose outputs with a Kalman filter. It is simple and modular but throws away information, because the camera's real evidence is individual feature bearings, not a finished pose. A tightly coupled system puts the raw feature reprojection residuals and the raw inertial residuals into one joint estimator over the full state. Tight coupling is uniformly more accurate and is the standard in every competitive system; the cost is a larger, nonlinear optimization that must run in real time.

The obstacle to tight coupling used to be the mismatch of rates: the camera produces a keyframe every 30 ms while the IMU produces a sample every 5 ms, so between two frames sit dozens of inertial readings. Re-integrating them from scratch each time the linearization point moves would be ruinous. IMU preintegration, introduced by Forster and colleagues, solves this by integrating the inertial measurements between two keyframes into a single relative-motion factor expressed on the \(SO(3)\) manifold, with an analytic correction for how that factor shifts when the bias estimate changes. The preintegrated measurement between keyframes \(i\) and \(j\) constrains the change in rotation, velocity, and position,

so the many raw samples collapse into one factor whose covariance is propagated once. This single idea is what let tightly coupled VIO become a real-time factor graph (the smoothing of Chapter 11) rather than a research curiosity.

Two estimator families: filtering and fixed-lag smoothing

Given tightly coupled residuals, two architectures dominate. The filtering family, exemplified by the Multi-State Constraint Kalman Filter (MSCKF), keeps a sliding window of recent camera poses in the EKF state and marginalizes each feature after using it to constrain those poses, which keeps computation linear in the number of features and bounded per step. The optimization family, exemplified by OKVIS, VINS-Mono, and VINS-Fusion, keeps an explicit sliding-window factor graph of recent keyframes and landmarks and runs a few Gauss-Newton iterations per keyframe, marginalizing old states into a prior to bound the window. Optimization re-linearizes and so is generally more accurate; filtering is lighter and more predictable on a tight compute budget. The choice is a genuine engineering tradeoff, not a settled winner, and both appear in shipping products.

One step precedes either estimator and quietly decides success: initialization. Before the filter or optimizer can run, the system must estimate the initial velocity, the gravity direction in the camera frame, the metric scale, and the initial gyroscope and accelerometer biases, typically by aligning a short visual-only structure-from-motion trajectory with the preintegrated inertial factors. Poor initialization (starting perfectly still, so the accelerometer sees only gravity and scale is unobservable) is the most common reason a VIO system diverges in the first two seconds. Practical systems demand a small excitation, a gentle wiggle, before they declare a valid pose.

import numpy as np

def preintegrate(gyro, accel, dt, bg, ba):
    """Minimal IMU preintegration between two keyframes (bias-corrected)."""
    dR = np.eye(3)                       # delta rotation  (SO(3))
    dv = np.zeros(3)                     # delta velocity  (body frame at i)
    dp = np.zeros(3)                     # delta position  (body frame at i)
    for w, a in zip(gyro, accel):
        w = w - bg; a = a - ba           # remove estimated biases
        theta = w * dt                   # small-angle rotation increment
        ang = np.linalg.norm(theta)
        K = _skew(theta / ang) if ang > 1e-9 else np.zeros((3, 3))
        dRi = np.eye(3) + np.sin(ang) * K + (1 - np.cos(ang)) * K @ K  # Rodrigues
        dp = dp + dv * dt + 0.5 * dR @ a * dt**2
        dv = dv + dR @ a * dt
        dR = dR @ dRi
    return dR, dv, dp                    # one factor, ready for the graph

def _skew(v):
    return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])

Listing 52.1 shows why preintegration is the load-bearing idea: the inner loop runs once per IMU sample, but the estimator that consumes it only sees one compact factor per keyframe, decoupling the fast inertial clock from the slow optimization clock.

When VIO is the right tool, and when it is not

The when is bounded by physics. VIO is the right choice for a device that moves through a texture-rich, adequately lit environment and needs a smooth, metric, self-contained pose without external infrastructure: phones, AR/VR headsets, indoor drones, warehouse robots. It is the wrong choice, alone, when the environment is dark, textureless (a blank corridor), or dominated by moving objects that violate the static-scene assumption, and when absolute global position is required, since VIO's global position and yaw are unobservable and drift without bound over long trajectories. The standard fixes are exactly the subjects ahead: add loop closure and a map to become full SLAM (Sections 52.2 onward), or fuse an absolute reference such as GNSS from Chapter 25 to pin down the unobservable directions. VIO is the drift-bounded local engine; the rest of this chapter is about anchoring it to a persistent world.

What's Next

In Section 52.2, we promote this drift-bounded front end into full simultaneous localization and mapping: ORB-SLAM3 adds a persistent feature map and loop closure that corrects the global position and yaw VIO leaves unobservable, and DROID-SLAM replaces hand-designed features with a differentiable dense front end. The preintegrated inertial factor built here carries straight into both.