Visual Odometry

Visual odometry (VO) estimates how a camera moves from the images it records, one frame at a time. For a camera rigidly attached to a vehicle, robot, drone, or headset, this is the platform’s ego-motion. The name, coined by Nistér, Naroditsky and Bergen (2004), is an analogy with wheel odometry: where wheel odometry integrates wheel turns, VO integrates the motions between consecutive frames (Scaramuzza and Fraundorfer, 2011). It is unaffected by wheel slip and works where GPS is unavailable.

VO is a form of motion estimation built on multi-view geometry. It differs from simultaneous localization and mapping (SLAM) in what it promises: VO keeps the trajectory locally consistent and lets errors accumulate; SLAM also recognizes places it has visited before (loop closure) and uses them to make the trajectory and map globally consistent (Scaramuzza and Fraundorfer, 2011).

Problem Definition

A camera captures images $I_0, \dots, I_n$. Let $C_k \in SE(3)$ be the pose of camera $k$, mapping its coordinates to those of the first camera, and $T_k = C_{k-1}^{-1} C_k$ the motion between frames $k-1$ and $k$. VO estimates each $T_k$ from the images and chains them (Scaramuzza and Fraundorfer, 2011):

$$C_k = C_{k-1}\,T_k, \qquad T_k = \begin{bmatrix} R_{k-1,k} & \mathbf{t}_{k-1,k} \\ \mathbf{0}^\top & 1 \end{bmatrix},$$

with $C_0$ usually the identity. Each pose is computed as its frame arrives, possibly refined over a short window. The usual assumptions are a calibrated camera, enough light and texture, a mostly static scene, and enough overlap between consecutive frames.

Input and Output

• Input: calibrated images from one camera, a stereo pair, or an RGB-D camera, often with inertial measurements.
• Output: a pose for every frame or keyframe, often with velocity, local 3D points, and uncertainty.

Variants

• Stereo VO. A stereo rig with a known baseline triangulates points at every step, giving metric scale. When the scene is far compared with the baseline, disparity becomes too small for reliable depth and stereo degenerates to the monocular case (Scaramuzza and Fraundorfer, 2011).
• Monocular VO. A single camera gives only bearings: the essential matrix fixes the direction of translation but not its length, so trajectory and map have no absolute scale.
• RGB-D VO. A metric depth map at every frame allows aligning 3D points, for example with iterative closest point, 3D–2D pose estimation, or direct intensity alignment.
• Visual-inertial odometry (VIO). An inertial measurement unit (IMU) measures acceleration and angular velocity at a higher rate than the camera; fusing it makes scale observable for a single camera and bridges short visual failures.

Challenges

• Drift. Chaining frame-to-frame estimates compounds their errors: under first-order error propagation, the covariance of $C_k$ only grows (Fraundorfer and Scaramuzza, 2012). Orientation errors are the most costly, because an early heading error bends the rest of the path: Scaramuzza and Fraundorfer (2011) report that Olson and colleagues found the error of stereo ego-motion alone growing superlinearly with distance, and linearly once an absolute orientation sensor was added.

• Scale ambiguity and scale drift. In monocular VO the first translation is given an arbitrary length, often 1. Each later translation needs a consistent relative scale, for example the median, over many point pairs, of the distance ratio between points triangulated from consecutive frame pairs,

  $$r = \frac{\lVert \mathbf{X}_{k-1,i} - \mathbf{X}_{k-1,j} \rVert}{\lVert \mathbf{X}_{k,i} - \mathbf{X}_{k,j} \rVert},$$

  or 3D–2D pose estimation against existing points (Scaramuzza and Fraundorfer, 2011). Each step adds error, so the scale itself drifts, as the SVO and DSO evaluations show (Forster et al., 2014; Engel et al., 2018). Absolute scale needs outside information: a known object size, motion constraints, or another sensor.

• Degeneracies. Pure rotation gives no parallax, so translation direction and depth are undefined; a planar scene can leave two essential matrices that fit every match (see the five-point algorithm); distant features constrain rotation but not translation.

• Outliers and appearance. Wrong matches and independently moving objects must be rejected, usually with RANSAC. Motion blur, low texture, rolling shutter, and exposure changes hurt both feature matching and direct methods.

Approaches

History

According to Scaramuzza and Fraundorfer (2011), ego-motion estimation from images began with Hans Moravec’s 1980 rover. It moved stop-and-go; at each stop a camera sliding on a rail took nine images, and the motion was the rigid transformation aligning the corners triangulated at consecutive stops. Much early work, motivated by planetary rovers, followed this stereo, 3D-to-3D scheme; NASA’s Spirit and Opportunity rovers used stereo VO on Mars from January 2004 (Fraundorfer and Scaramuzza, 2012).

Nistér, Naroditsky and Bergen (2004) presented a real-time hypothesize-and-test system for a stereo head or a single camera. Scaramuzza and Fraundorfer (2011) describe it as the first real-time, long-run VO implementation with robust outlier rejection: Harris corners were detected independently in every frame and matched, which avoids the drift of correlation tracking; motion came from 3D-to-2D pose estimation rather than aligning triangulated point sets; and minimal solvers, the five-point algorithm for one camera, ran inside RANSAC.

The feature-based pipeline

Scaramuzza and Fraundorfer (2011) describe the usual pipeline:

1. Associate features by matching detections or tracking them with KLT.
2. Estimate the motion $T_k$:
   • 2D–2D: the essential matrix from the five-point algorithm in RANSAC, giving $R$ and the direction of $\mathbf{t}$, which is then rescaled.
   • 3D–2D: perspective-n-point against points triangulated earlier or from the stereo pair at $k-1$, refined on reprojection error.
   • 3D–3D: aligning point sets triangulated at both times. According to Scaramuzza and Fraundorfer (2011), Nistér et al. (2004) found 3D–2D greatly superior for stereo, because triangulated points are much more uncertain in depth, and 3D–3D alignment minimizes those 3D errors directly.
3. Triangulate new points as old ones leave the view.
4. Select keyframes. Nearby frames give poorly constrained points, so frames are skipped until point uncertainty is low enough.
5. Refine locally with windowed bundle adjustment over the last $m$ keyframes, which reduces drift (Fraundorfer and Scaramuzza, 2012).

Because a 3D–2D hypothesis needs three correspondences against five for 2D–2D, RANSAC is much faster, which Scaramuzza and Fraundorfer (2011) give as the reason 3D–2D is more common even in monocular systems.

Direct and semi-direct methods

Feature-based, or indirect, methods turn images into point positions and minimize a geometric error. Direct methods minimize a photometric error on the intensities themselves (see brightness constancy). For a pixel $\mathbf{p}$ with inverse depth $d_\mathbf{p}$ in frame $i$, observed in frame $j$, the error used by DSO is, in simplified form (dropping its exposure-time and affine brightness terms),

$$E_{\mathbf{p}j} = \sum_{\mathbf{q} \in \mathcal{N}_\mathbf{p}} w_\mathbf{q} \,\Big\lVert I_j\big[\pi\big(R_{ji}\,\pi^{-1}(\mathbf{q}, d_\mathbf{p}) + \mathbf{t}_{ji}\big)\big] - I_i[\mathbf{q}] \Big\rVert_\gamma,$$

where $\pi$ projects to the image, $\pi^{-1}$ back-projects a pixel at the given inverse depth, $\mathcal{N}_\mathbf{p}$ is a small pixel pattern around $\mathbf{p}$, $w_\mathbf{q}$ down-weights high-gradient pixels, and $\lVert\cdot\rVert_\gamma$ is the Huber norm (Engel et al., 2018). Direct methods can use any pixel with gradient, including edges, but need a good initial guess because the photometric error has a small region of attraction (Qin, Li and Shen, 2018).

Direct Sparse Odometry (DSO; Engel, Koltun and Cremers, 2018) samples pixels evenly across regions with gradient and optimizes poses, inverse depths, and affine brightness parameters over a sliding window of up to seven keyframes with Gauss–Newton, marginalizing old ones. SVO (Forster, Pizzoli and Scaramuzza, 2014) is semi-direct: it initializes each pose by photometric alignment of small patches around projected map points, without extracting features in every frame, then refines pose and structure on reprojection error. New points enter the map once a probabilistic depth filter converges.

Visual-inertial odometry

Loosely coupled fusion combines the IMU with a vision-only pose estimate, often in an EKF; tightly coupled fusion estimates from camera and IMU measurements jointly (Qin, Li and Shen, 2018). The multi-state constraint Kalman filter (MSCKF; Mourikis and Roumeliotis, 2007) is a tightly coupled extended Kalman filter whose state holds a window of recent camera poses. Its measurement model constrains all poses that saw a feature without adding the feature to the state, so the cost is linear in the number of features. VINS-Mono (Qin, Li and Shen, 2018) instead solves a sliding-window nonlinear least-squares problem over pre-integrated IMU measurements and feature observations. With one camera and an IMU, metric scale, roll, and pitch become observable, but only with enough acceleration, so initialization needs care. Drift remains in the 3D position and in yaw about gravity, the four degrees of freedom of VINS-Mono’s pose-graph optimization.

Learned visual odometry

DeepVO (Wang et al., 2017) regresses poses from monocular video with a recurrent convolutional network trained on ground-truth poses; the authors reported performance competitive with conventional methods on KITTI. Zhou et al. (2017) trained depth and pose networks without ground truth, using view synthesis, reconstructing one frame from another through the predictions, as supervision.

Example

The script simulates stereo VO along a 1 km curved drive, one frame per meter, past random landmarks. At each frame it triangulates the matches from the previous stereo pair, estimates the new pose by Gauss–Newton minimization of reprojection error (the refinement step of PnP), and chains the motions. Correspondences are known and outlier-free; the only error source is 0.5 px of pixel noise.

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

rng = np.random.default_rng(0)
f, cx, cy, W, H = 700.0, 640.0, 190.0, 1280, 380   # pinhole intrinsics (pixels)
b, sigma = 0.5, 0.5                                 # stereo baseline (m), pixel noise

# Ground-truth camera-to-world poses C_k = (R, c): a 1 km drive, one frame per meter.
# Camera axes: x right, y down, z forward; the path curves gently in the x-z plane.
s = np.arange(1061.0)                                # extra 60 m so the last frames see landmarks
yaw = 0.4 * np.sin(s / 120.0)
centers = np.column_stack([np.cumsum(np.sin(yaw)), np.zeros_like(s), np.cumsum(np.cos(yaw))])
gt = [(Rotation.from_rotvec([0, a, 0]).as_matrix(), c) for a, c in zip(yaw[:1001], centers)]

# Landmarks scattered on both sides of the road
idx = rng.integers(0, len(s), 30000)
side = rng.choice([-1.0, 1.0], len(idx)) * rng.uniform(3, 20, len(idx))
normal = np.column_stack([np.cos(yaw[idx]), np.zeros(len(idx)), -np.sin(yaw[idx])])
pts = centers[idx] + side[:, None] * normal
pts[:, 1] = rng.uniform(-4, 1.5, len(idx))
pts[:, [0, 2]] += rng.uniform(-0.5, 0.5, (len(idx), 2))

def observe(R, c):
    """Noisy left/right pixel coordinates of the landmarks visible in a rectified stereo pair."""
    Xc = (pts - c) @ R                                  # world -> left-camera frame
    vis = (Xc[:, 2] > 4) & (Xc[:, 2] < 40)
    uL = f * Xc[:, 0] / Xc[:, 2] + cx
    v = f * Xc[:, 1] / Xc[:, 2] + cy
    vis &= (uL > 0) & (uL < W) & (v > 0) & (v < H)
    ids = np.flatnonzero(vis)
    n = len(ids)
    uL, v = uL[ids] + sigma * rng.normal(size=n), v[ids] + sigma * rng.normal(size=n)
    uR = f * (Xc[ids, 0] - b) / Xc[ids, 2] + cx + sigma * rng.normal(size=n)
    return ids, uL, v, uR

def stereo_points(uL, v, uR):
    """Triangulate from disparity: Z = f b / d."""
    Z = f * b / np.maximum(uL - uR, 1e-3)
    return np.column_stack([(uL - cx) * Z / f, (v - cy) * Z / f, Z])

def pnp_gauss_newton(X, uv, R, t, iters=10):
    """Refine X_cam = R X + t by minimizing reprojection error (3D-2D motion)."""
    for _ in range(iters):
        Y = X @ R.T + t
        r = (np.column_stack([f * Y[:, 0] / Y[:, 2] + cx, f * Y[:, 1] / Y[:, 2] + cy]) - uv).ravel()
        J = np.zeros((len(r), 6))
        for k in range(6):                              # numerical Jacobian w.r.t. (rotvec, t)
            d = np.zeros(6); d[k] = 1e-6
            R2 = Rotation.from_rotvec(d[:3]).as_matrix() @ R
            Y2 = X @ R2.T + t + d[3:]
            r2 = (np.column_stack([f * Y2[:, 0] / Y2[:, 2] + cx, f * Y2[:, 1] / Y2[:, 2] + cy]) - uv).ravel()
            J[:, k] = (r2 - r) / 1e-6
        dx = np.linalg.lstsq(J, -r, rcond=None)[0]
        R = Rotation.from_rotvec(dx[:3]).as_matrix() @ R
        t = t + dx[3:]
    return R, t

# Stereo VO: triangulate at frame k-1, pose frame k from 3D-2D matches, chain the motions.
est, n_matches = [gt[0]], []
prev = observe(*gt[0])
Rm, tm = np.eye(3), np.zeros(3)                         # constant-motion initial guess
for k in range(1, len(gt)):
    cur = observe(*gt[k])
    common, i0, i1 = np.intersect1d(prev[0], cur[0], return_indices=True)
    X = stereo_points(prev[1][i0], prev[2][i0], prev[3][i0])
    uv = np.column_stack([cur[1][i1], cur[2][i1]])
    n_matches.append(len(common))
    Rm, tm = pnp_gauss_newton(X, uv, Rm, tm)            # maps frame k-1 coords to frame k
    Rp, cp = est[-1]
    # Camera k in frame k-1 has rotation Rm^T and center -Rm^T tm; compose with C_{k-1}
    est.append((Rp @ Rm.T, cp + Rp @ (-Rm.T @ tm)))
    prev = cur

def rel(A, B):
    """Relative motion A^{-1} B of two camera-to-world poses, as a 4x4 matrix."""
    T = np.eye(4); T[:3, :3] = A[0].T @ B[0]; T[:3, 3] = A[0].T @ (B[1] - A[1])
    return T

dist = np.concatenate([[0], np.cumsum(np.linalg.norm(np.diff(centers[:len(gt)], axis=0), axis=1))])
print(f"frames: {len(gt)}, path length: {dist[-1]:.0f} m, "
      f"matches per frame: {min(n_matches)} to {max(n_matches)}")
for k in [100, 250, 500, 1000]:
    e = np.linalg.norm(est[k][1] - gt[k][1])
    print(f"after {dist[k]:5.0f} m: position error {e:5.2f} m ({100 * e / dist[k]:.2f}% of distance)")

# KITTI-style errors: relative pose error over all segments of length L, divided by L
t_err, r_err = [], []
for L in range(100, 900, 100):
    te, re = [], []
    for i in range(0, len(gt), 10):
        j = np.searchsorted(dist, dist[i] + L)
        if j >= len(gt):
            break
        E = np.linalg.inv(rel(gt[i], gt[j])) @ rel(est[i], est[j])
        te.append(np.linalg.norm(E[:3, 3]) / L)
        re.append(np.degrees(np.arccos(np.clip((np.trace(E[:3, :3]) - 1) / 2, -1, 1))) / L)
    t_err += te; r_err += re
    print(f"segments of {L} m: translational error {100 * np.mean(te):.2f}%")
print(f"KITTI-style average: {100 * np.mean(t_err):.2f}% translation, "
      f"{np.mean(r_err):.4f} deg/m rotation")

Output:

frames: 1001, path length: 1000 m, matches per frame: 654 to 808
after   100 m: position error  0.30 m (0.30% of distance)
after   250 m: position error  3.01 m (1.21% of distance)
after   500 m: position error 11.35 m (2.27% of distance)
after  1000 m: position error 45.44 m (4.54% of distance)
segments of 100 m: translational error 0.28%
segments of 200 m: translational error 0.74%
segments of 300 m: translational error 1.20%
segments of 400 m: translational error 1.62%
segments of 500 m: translational error 2.01%
segments of 600 m: translational error 2.40%
segments of 700 m: translational error 2.83%
segments of 800 m: translational error 3.34%
KITTI-style average: 1.40% translation, 0.0057 deg/m rotation

With hundreds of correct matches per frame, the end-point error still grows from 0.3 m after 100 m to 45 m after 1 km, and the KITTI-style error grows with segment length. The cause is heading: about 6° of accumulated yaw by the end swings the path sideways. Substituting the true rotations into the same script (not shown) gives about 0.3% at every segment length, the residue of noisy stereo depth. Windowed bundle adjustment or an IMU would slow the growth.

Datasets and Benchmarks

• KITTI odometry (Geiger, Lenz and Urtasun, 2012): 22 stereo driving sequences totaling 39.2 km at 10 frames per second; 00–10 have GPS/IMU ground truth, and 11–21 are scored by the benchmark server, which forbids manual loop closure.
• TUM RGB-D (Sturm et al., 2012): 39 Kinect sequences, handheld or robot-mounted, with color and depth at 30 Hz and motion-capture ground truth at 100 Hz.
• EuRoC MAV (Burri et al., 2016): eleven drone sequences with synchronized stereo images and IMU data, and ground truth from a laser tracker or motion capture, from slow, well-lit flights to dynamic ones with motion blur and poor illumination.

Evaluation Metrics

End-point error is misleading because an early rotation error moves the end point far more than a late one (Geiger et al., 2012; Sturm et al., 2012), so benchmarks compare relative motions. For estimated poses $\hat{C}$ and ground truth $C$, the error of the motion from frame $i$ to frame $j$ is

$$E_{ij} = \big(C_i^{-1} C_j\big)^{-1}\big(\hat{C}_i^{-1}\hat{C}_j\big).$$

• KITTI translational and rotational error (Geiger et al., 2012): the translation and rotation angle of $E_{ij}$ divided by the subsequence length, averaged over all subsequences of 100, 200, …, 800 m, in percent and degrees per meter. Until October 2013 the lengths were 5, 10, 50, 100, …, 400 m; they were changed because GPS/IMU ground-truth errors biased the short subsequences.
• Relative pose error (RPE) (Sturm et al., 2012): the RMS translational part of $E_{i,i+\Delta}$ for a fixed interval $\Delta$, the drift per frame or per second, suited to VO.
• Absolute trajectory error (ATE) (Sturm et al., 2012): the RMS position error after least-squares rigid alignment of the two trajectories, a measure of global consistency. Monocular results are aligned with a similarity transformation, Sim(3), that also fixes scale, as in the EuRoC results of Engel et al. (2018).

Applications

• Planetary rovers: the Mars Exploration Rovers used stereo VO to approach targets, drive safely near obstacles on slopes, and check for wheel slip (Fraundorfer and Scaramuzza, 2012).
• Driving and mobile robots: motion estimation alongside wheel and LiDAR odometry, and where GPS is blocked.
• Drones, phones, and headsets: VIO for GPS-denied flight and for keeping AR and VR content fixed in the world.
• SLAM: the tracking front end that loop closure later corrects.

Open Problems

Without an absolute reference, drift is inevitable. Robustness to low texture, fast motion, night, weather, and moving objects lags behind benchmark accuracy. VIO depends on accurate time synchronization, camera–IMU calibration, and sufficiently exciting motion. How best to combine learned components with geometric pipelines, and how well they transfer to new cameras and scenes, remain open.