Visual & Visual-Inertial Odometry

Chapter 1: The VO Pipeline — Front-End and Back-End

A complete VO system has two conceptually separate parts. The front-end touches raw pixels: it detects features, tracks them across frames, and computes noisy relative-pose measurements. The back-end takes those measurements and finds the globally consistent trajectory via optimization (or filtering). The split matters because the front-end is fast but noisy; the back-end is slow but accurate.

Front-end: feature tracking and relative pose

At time k the camera captures frame I_k. The front-end detects corners with Harris or Shi-Tomasi (L6), then tracks them into frame I_k+1 with KLT optical flow. You now have a set of point correspondences {(p_i, p'_i)} — the same 3D point seen from two camera positions. Plug those into the 5-point algorithm (or 8-point with normalization) to estimate the essential matrix E = [t]_× R (L7). RANSAC (L8) removes outliers. Decompose E into (R, t) — four hypotheses, cheirality picks the right one.

Back-end: trajectory building

The back-end chains relative poses. Given relative transform T_k,k+1 (pose of frame k+1 expressed in frame k), the absolute pose of frame k+1 in the world frame is:

where T_w,k is the world-to-camera transform at frame k. This is SE(3) matrix multiplication — the on-manifold composition from L10.

Worked example: chaining two relative poses

Suppose the drone starts at the world origin T₀ = I₄. Between frames 0 and 1 it moves forward 1 m and turns 5° right. Between frames 1 and 2 it moves forward 1 m with no turn. In the (R | t) notation:

python
import numpy as np
from scipy.spatial.transform import Rotation

# Relative pose: frame 0 → 1 (5° yaw right, 1 m forward)
R01 = Rotation.from_euler('z', -5, degrees=True).as_matrix()
t01 = np.array([1.0, 0.0, 0.0])  # 1 m forward in frame-0 coords

# Relative pose: frame 1 → 2 (no turn, 1 m forward)
R12 = np.eye(3)
t12 = np.array([1.0, 0.0, 0.0])

# Chain: world pose of frame 2
# T_world_frame2 = T_world_frame0 @ T_01 @ T_12
# Starting at identity (T_world_frame0 = I)

# Position of frame 1 in world
p1 = t01  # = [1, 0, 0]

# Position of frame 2 in world = p1 + R01 @ t12
p2 = p1 + R01 @ t12
print("Frame 1 position:", np.round(p1, 4))   # [1.0, 0.0, 0.0]
print("Frame 2 position:", np.round(p2, 4))
# [1 + cos(-5°), 0 + sin(-5°), 0]
# = [1 + 0.9962, 0 + (-0.0872), 0]
# = [1.9962, -0.0872, 0.0]
# The drone veered slightly right on the second step — the 5° turn propagated!

This is the essence of the back-end: each step applies the new relative rotation R_k before adding the translation, so previous rotations shape all future translations. Errors in early rotations deviate all later positions.

Chapter 2: Drift — Why Small Errors Become Big Problems

Every relative-pose estimate carries a small error: the features matched had sub-pixel noise, RANSAC didn't find every inlier, the essential matrix decomposition had numerical error. In isolation, each error is tiny. But the back-end multiplies SE(3) transforms — and multiplication compounds errors multiplicatively, not additively.

The drift math

Suppose each step introduces a position error of ε meters (absolute) along the direction of motion. After N steps of length d, the position error grows. In the best case (errors partially cancel): O(ε√N). In the worst case (all errors in the same direction, e.g., systematic heading bias): O(εN). In practice VO drift is typically 0.1–2% of total path length — the error grows roughly linearly with distance traveled.

where ε_rel is the fractional per-step error (typically 0.5–1% for a good front-end).

Worked numbers

Path = 200 m loop. Per-step error ε_rel = 1%. Expected drift ≈ 2 m. The drone started at a door, flew 200 m around a room, and estimates it's 2 m from where it started. The loop doesn't close. That's drift.

Keyframes reduce drift (a little)

Running the optimization back-end over a sliding window of recent keyframes (selected frames that maximize information gain) helps. Bundle adjustment over N keyframes re-estimates all poses and map points jointly, which spreads and reduces error compared to purely sequential pose chaining. But it doesn't eliminate drift — it only slows its growth. The trajectory still drifts because the window doesn't extend back to the start.

Chapter 3: Scale Ambiguity — the Monocular Blind Spot

The 5-point algorithm recovers R and t from pixel correspondences. R is determined fully. But t is determined only up to a positive scale factor: if (R, λt) is consistent with the observed correspondences for any λ > 0, so is (R, t). You can see this directly from the essential matrix: E = [t]_×R = [λt]_×R for any λ — the homogeneous constraint never pins the scale.

This is not a numerical issue — it's a fundamental geometric one. A single camera is a projective device: it collapses 3D onto 2D and throws away all absolute depth information. Without knowing how big the scene is, you can't know how far you moved.

The scale factor propagates

In sequential VO, you can chain the up-to-scale relative translations if you assume a fixed scale: set |t_0,1| = 1 (the baseline unit) and express all subsequent translations in the same unit. This gives a metrically consistent but incorrectly scaled trajectory. If you flew 10 m total but your unit baseline was actually 0.1 m, your trajectory reports 10 "units" but doesn't know those units are 0.1 m each. The shape is correct; the size is unknown.

How scale is recovered in practice

Chapter 4: The IMU — Fast, Metric, and Drifting

An Inertial Measurement Unit (IMU) is a tiny chip containing two sensors: an accelerometer and a gyroscope. Together they measure the complete 6-DoF motion of the body they're attached to, at very high rate (100–400 Hz typically). On a phone, a quadrotor, a car — every device with autonomous motion has at least one.

What the accelerometer measures

The accelerometer measures specific force: the sum of all forces except gravity, divided by mass. In other words, it measures acceleration relative to free-fall. If the IMU is at rest on a table, the accelerometer reads g = [0, 0, 9.81] m/s² upward (the table pushes back against gravity). If it's in free fall, it reads zero. To get true linear acceleration you subtract gravity:

where R is the current orientation (to rotate g from world to body frame) and g_world = [0, 0, 9.81]^T m/s².

What the gyroscope measures

The gyroscope measures angular velocity ω in the body frame — the instantaneous rotation rate around each body axis. Integrate ω over time to get orientation change. Compose with the current orientation to update it:

This is the on-manifold SO(3) update from L10: take a step ωΔt in the tangent space, exponentiate it onto the manifold, compose with the current rotation.

Bias and noise — the killers

Every IMU reading is corrupted by two things:

The bias is insidious. If the accelerometer has a bias of b_a = 0.1 m/s², then integrating once to get velocity gives a velocity error of b_aΔt per step. Integrating again to get position gives a position error of ½b_aΔt². Over 10 seconds at 0.1 m/s² bias, the position error is 5 m. This is why IMU-only dead reckoning fails within seconds.

Chapter 5: IMU Dead-Reckoning — Fast but Blind

Dead reckoning means estimating your position by integrating known velocities or accelerations forward from a known starting point — no landmarks, no GPS, just pure integration. With an IMU, you integrate twice: acceleration to velocity, velocity to position. It's conceptually simple and blazing fast, but the errors are catastrophic on their own.

The three integration equations

At time step k, with Δt between samples, the IMU propagates the state (R, v, p) — orientation, velocity, position — as:

where g = [0, 0, −9.81]^T m/s² in world frame. The key insight: every line depends on R_k, which accumulates its own errors. Orientation error pollutes velocity, which pollutes position.

Worked drift example: 1D car

Car drives straight for 10 s at 5 m/s (should end at 50 m). Accelerometer reads 0 (constant speed). But bias b_a = 0.1 m/s²:

• After 1 s: velocity estimate = 0.1 m/s (wrong by 0.1); position error = ½×0.1×1 = 0.05 m
• After 5 s: velocity error = 0.5 m/s; position error = ½×0.1×25 = 1.25 m
• After 10 s: velocity error = 1.0 m/s; position error = ½×0.1×100 = 5 m

The car ends up at an estimated 55 m instead of the true 50 m — 10% error after just 10 seconds, with only a 0.1 m/s² bias. And real IMU bias is often larger.

Orientation-error coupling

Now add a gyroscope bias b_g = 0.01 rad/s. After 10 s, the orientation error is 0.1 rad ≈ 5.7°. In the velocity integration, R_k rotates the accelerometer reading into world frame. A 5.7° orientation error means gravity (9.81 m/s²) leaks into the horizontal acceleration at sin(5.7°) × 9.81 ≈ 0.97 m/s². This is 10× larger than the original accelerometer bias! Orientation error catastrophically amplifies position drift.

Chapter 6: Why Fuse Vision + IMU — Complementary Failure Modes

The camera and the IMU fail in completely different ways. This is the key observation that makes VIO so powerful: their weaknesses are each other's strengths.

The camera provides three critical gifts to the IMU

1. Bias correction: the optimizer estimates IMU bias b_a, b_g jointly with the trajectory. When the camera pins the trajectory accurately, the optimizer can infer what bias would explain the discrepancy between visual and inertial motion — and correct it. 2. Long-term stability: the camera's feature matches provide absolute constraints that prevent unbounded growth of IMU error. 3. Orientation observability: yaw is not observable from the IMU alone (spinning in place looks the same as being still to an accelerometer). Camera features give yaw information via the homography/essential matrix.

Loosely coupled vs. tightly coupled

Loosely coupled VIO: run VO separately on the camera (output: relative pose T_k,k+1 with covariance). Run IMU integration separately (output: predicted T_k,k+1^IMU). Fuse the two estimates in an EKF or factor graph. Simple but suboptimal — errors in VO are opaque to the IMU subsystem.

Tightly coupled VIO: the optimizer jointly minimizes reprojection errors (from camera feature tracks) and IMU preintegration residuals in a single factor graph over poses, velocities, biases, and 3D landmark positions. This is harder to implement but produces the best accuracy. MSCKF and OKVIS, ORB-VIO, VINS-Mono are all tightly coupled.

Chapter 7: IMU Preintegration — One Constraint from Many Readings

Between two camera keyframes there are typically many IMU readings — if the camera runs at 30 Hz and the IMU at 200 Hz, that's about 6–7 IMU samples per camera interval. The back-end optimizer works in terms of keyframe poses. How do you incorporate all those IMU readings efficiently?

Naive approach: re-integrate all IMU readings from the first keyframe whenever you want to compute the constraint between keyframes i and j. Problem: if the optimizer changes the estimate at keyframe i (as it will, repeatedly), you'd have to redo the entire integration chain — an O(N) cost per optimization step over N IMU samples.

Preintegration: recognize that the relative motion between two keyframes can be summarized as a single measurement — the preintegrated IMU measurement — that depends only on the IMU readings between those keyframes, not on the absolute pose at the start. When the optimizer changes the keyframe poses, the preintegrated constraint stays fixed; only the residual changes. This is O(1) per optimization step.

What the preintegrated measurement looks like

For IMU readings ω_k (gyro) and a_k (accel) at times t_k between keyframes i and j:

These are computed once in preprocessing. The residuals in the factor graph are then:

Bias updates without re-integration

When the optimizer updates the bias estimate (b_a, b_g), you'd normally need to redo the preintegration. But using the first-order Jacobians ∂ΔR⁄∂b_g, ∂Δv⁄∂b_a, ∂Δp⁄∂b_a (computed during preintegration), you can correct the preintegrated quantities cheaply with a linear approximation — no full re-integration needed unless the bias changes dramatically.

Chapter 8: Showcase — VIO Simulator

Watch ground truth, VO-only, and VIO trajectories side by side. Toggle vision dropouts and IMU noise to see them cover each other's weaknesses.

Chapter 9: Connections & Cheat Sheet

You've now built the complete story from raw pixels and IMU readings to a fused VIO system. Here's how everything fits together.

Core concepts — quick reference

Where VO/VIO fits in the VNAV curriculum

Open-source VIO systems to study

"Every measurement tells you something about the world. Every integration multiplies your ignorance. The art of state estimation is to measure as often as possible and integrate as rarely as possible." — paraphrasing Stergios Roumeliotis