Visual SLAM: Mapping While Moving

"Tourists get to reconstruct their vacation after they fly home. I have to know exactly where I am while the drone is still dodging the ceiling fan."

A Mildly Stressed SLAM System

Everything in this chapter so far was offline: all images available up front, minutes or hours of compute acceptable, and the answer delivered once, at the end. A robot inverts every assumption. Frames arrive one at a time and the camera's pose is needed now, within a frame budget of about 33 ms, because a controller is waiting on it; compute and memory are bounded forever, even as the trajectory grows without limit; and there is no second pass: decisions about the past must be revised live, as new evidence arrives. SLAM is the family of designs that meets these constraints, and visual SLAM is its camera-only branch. Remarkably, it concedes very little geometry: the front end is still feature matching from Chapter 10, pose estimation is still PnP, refinement is still bundle adjustment, only scheduled with extreme care about what gets computed when.

1. What the Deadline Changes Beginner

The first casualty of real time is the idea of treating every frame equally. At 30 fps, consecutive frames are nearly identical; reconstructing from all of them wastes compute on redundant views with near-zero baseline, the degenerate-motion trap of Section 14.2. The fix is the keyframe: a frame promoted to the permanent map only when it adds information, typically when tracking quality against the current map drops below a threshold or the view has translated enough since the last keyframe. Everything in between is localized against the map and discarded. The second casualty is the single computation loop, replaced by threads running at different rates. This architecture, introduced by PTAM in 2007 and matured by the ORB-SLAM line, is now standard; Figure 14.4.1 shows it.

The covisibility graph named inside the map deserves a word: it links keyframes that observe shared map points, generalizing the match graph of Section 14.1 to the online setting. Its role is locality. "The local map" that tracking matches against, and that local BA refines, is a neighborhood in this graph, not a radius in space, which is exactly right when trajectories cross themselves or hover in place.

2. A Minimal Visual Odometry Loop Intermediate

Strip SLAM of its map, keyframes, and loop closure and what remains is visual odometry (VO): chain relative poses frame to frame and integrate them into a trajectory. It is the natural first build, and its failure is the best motivation for everything SLAM adds. The loop below runs the two-view machinery of Chapter 13 on consecutive frames of a driving video (intrinsics here are from the KITTI benchmark's left camera) and composes the results.

import cv2
import numpy as np

K = np.array([[718.856, 0.0, 607.193],      # KITTI sequence 00, left gray camera
              [0.0, 718.856, 185.216],
              [0.0, 0.0, 1.0]])

orb = cv2.ORB_create(3000)
bf = cv2.BFMatcher(cv2.NORM_HAMMING)
cap = cv2.VideoCapture("kitti00.mp4")

ok, frame = cap.read()
prev = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
kp_p, des_p = orb.detectAndCompute(prev, None)

pose = np.eye(4)                            # camera-to-world, starts at origin
trail = [pose[:3, 3].copy()]

while True:
    ok, frame = cap.read()
    if not ok:
        break
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    kp, des = orb.detectAndCompute(gray, None)
    knn = bf.knnMatch(des_p, des, k=2)
    good = [m for m, n in knn if m.distance < 0.8 * n.distance]
    if len(good) >= 60:                      # else: skip frame, keep last pose
        p0 = np.float32([kp_p[m.queryIdx].pt for m in good])
        p1 = np.float32([kp[m.trainIdx].pt for m in good])
        E, _ = cv2.findEssentialMat(p1, p0, cameraMatrix=K,
                                    method=cv2.USAC_MAGSAC, threshold=1.0)
        _, R, t, _ = cv2.recoverPose(E, p1, p0, K)
        step = np.eye(4)
        step[:3, :3], step[:3, 3] = R, t.ravel()   # |t| = 1: scale unknown!
        pose = pose @ step                   # compose into the global trajectory
    trail.append(pose[:3, 3].copy())
    kp_p, des_p = kp, des

trail = np.array(trail)
print(f"{len(trail)} poses, net displacement {np.linalg.norm(trail[-1]):.1f} units")
# 4541 poses, net displacement 18.3 units  (ground truth: the loop returns to 0!)

Two structural flaws hide in this loop, and both are instructive. First, scale: recoverPose returns a unit-length translation every frame, because monocular geometry cannot know metric scale (Chapter 13's lesson). Real monocular systems estimate relative scale by triangulating landmarks and tracking them across multiple steps, yet even then scale slowly drifts, a distinctly monocular disease. Second, compounding: each frame's small rotation error rotates all future displacement; errors integrate, and the trajectory bends away from truth without bound. VO with map points and windowed optimization can slow the bleed. Only recognizing a previously seen place can stop it.

3. Loop Closure: Recognizing Where You Have Been Intermediate

Loop closure is two problems chained. Detection is image retrieval under a time budget: does the current keyframe look like any old keyframe? The classical solution embeds each keyframe as a bag-of-visual-words vector over quantized binary descriptors (DBoW2 being the standard library), so candidate retrieval is an inverted-file lookup costing milliseconds even against thousands of keyframes; this is the same retrieval idea that Chapter 16 develops for recognition at large. Every candidate then faces geometric verification, a full relative-pose estimation with RANSAC, because perceptual aliasing (two identical corridors) makes appearance-only matches treacherous in exactly the way Section 14.1's doppelganger edges were.

Correction is the second problem: the verified loop says keyframe 980 sits, say, 0.4 m from keyframe 12, while the integrated chain claims 9 m. The error must be redistributed over the whole loop, and running full bundle adjustment over every keyframe and point is too slow for the moment of closure. The standard tool is pose-graph optimization: forget the points temporarily, keep only keyframe poses $T_i \in \mathrm{SE}(3)$ as variables, with every odometry edge and the new loop edge contributing a measured relative pose $\hat{T}_{ij}$, and minimize

$$ \sum_{(i,j)} \left\lVert \mathrm{Log}\!\left( \hat{T}_{ij}^{-1} \, T_i^{-1} T_j \right) \right\rVert^2_{\Sigma_{ij}}, $$

where $\mathrm{Log}$ maps a pose error to a 6-vector and $\Sigma_{ij}$ weights each edge by its confidence. The optimization bends the trajectory smoothly so the loop meets itself, after which a background global BA re-polishes points and poses together. Monocular systems optimize over similarity transforms $\mathrm{Sim}(3)$ instead, with a 7th scale parameter per pose, so that accumulated scale drift is absorbed around the loop too. Figure 14.4.2 shows the before and after.

4. Sensors: Monocular, Stereo, RGB-D, Inertial Intermediate

The sensor suite determines which of SLAM's diseases you contract. Monocular is the minimal deployment: no scale, scale drift, and fragile initialization, but any camera works. Stereo fixes scale outright, since the calibrated baseline of Chapter 13 makes every triangulation metric, and it initializes instantly from a single stereo pair. RGB-D sensors hand you per-pixel depth indoors, removing triangulation entirely from the critical path (their structured-light and time-of-flight ranging is sensor territory covered alongside Chapter 1's imaging pipeline). Visual-inertial systems (VIO) fuse an IMU, whose accelerometer makes scale and gravity observable and whose high-rate gyro carries tracking through motion blur, fast rotation, and textureless moments that starve any pure camera system. VIO is why a phone-based AR session (ARKit, ARCore) keeps virtual furniture glued to your floor while you whip the phone around: the camera corrects the IMU's drift, the IMU bridges the camera's failures, and each sensor's weakness is the other's strength. Production systems like ORB-SLAM3 support all of these modes over the same keyframe-and-graph backbone.