Week 12 — Simultaneous Localization and Mapping (SLAM) Day 3 of 7
Why This Matters
Visual SLAM answers a deceptively simple question: where am I, and what does the world look like? A camera is the cheapest sensor on any mobile robot. A 640×480 global-shutter camera costs about $15 in volume. It gives you 307,200 pixels at 30 Hz. No spinning LiDAR, no structured light, no time-of-flight. If you can turn those pixels into a metrically consistent map and a 6-DoF pose, you have localization for delivery robots, drones, and AR headsets without paying for a Velodyne.
The failure mode without good visual SLAM is brutal. A warehouse AMR that loses localization for 1.2 seconds at 1.5 m/s travels 1.8 m blind. That is enough to hit a rack leg, drop a pallet, or trigger an e-stop that costs 40 minutes of throughput. Drift compounds. A pure visual odometry system drifts roughly 1–3% of distance traveled. Over a 200 m patrol loop, that is 2–6 m of accumulated error. The map folds onto itself. Loop closure is the only thing that pulls it back.
ORB-SLAM matters because it was the first widely adopted feature-based visual SLAM system that ran in real time on a CPU, handled monocular, stereo, and RGB-D input, and produced a globally consistent map. ORB-SLAM2 (2017) and ORB-SLAM3 (2021) are still the reference baselines that every new visual SLAM paper compares against. If you work on mobile robots, you will either deploy it or debug something that claims to beat it. You need to understand its internals: the feature extractor, the three-thread architecture, the covisibility graph, and the bundle adjustment that ties it together.
Core Concepts
ORB Features: Why This Extractor
ORB stands for Oriented FAST and Rotated BRIEF. It is a two-part pipeline. First, FAST-9 corner detection with a pyramid. Second, a binary descriptor called rBRIEF.
FAST-9 works by testing a ring of 16 pixels around a candidate center. If 9 contiguous pixels are all brighter or all darker than the center by a threshold (typically 20 intensity levels), the point is a corner. This is cheap. On a 2.4 GHz ARM core, ORB-SLAM extracts about 1,000 keypoints from a 640×480 image in 8–12 ms.
The descriptor is where ORB earns its keep. BRIEF compares 256 pixel-pairs inside a 31×31 patch and writes one bit per comparison. The result is a 256-bit descriptor, or 32 bytes. Compare that to SIFT’s 128 floats (512 bytes) or SURF’s 64 floats. Matching uses Hamming distance, which on modern CPUs runs as a single POPCNT instruction per 64-bit word. A 256-bit comparison costs 4 POPCNT ops. You can brute-force match 1,000 query descriptors against 10,000 map descriptors in under 10 ms.
The “oriented” part fixes BRIEF’s rotation weakness. ORB computes the intensity centroid of the patch and uses the vector from patch center to centroid as the dominant orientation. Descriptors are rotated to compensate. This gives ORB invariance to in-plane rotation up to about 45°, which matters when a robot yaws.
| Property | ORB | SIFT | SURF | AKAZE |
|---|---|---|---|---|
| Descriptor size | 32 bytes | 512 bytes | 256 bytes | 61 bytes |
| Match metric | Hamming | L2 | L2 | Hamming |
| Extract time (VGA) | ~10 ms | ~120 ms | ~60 ms | ~45 ms |
| Rotation invariant | Yes (≤45°) | Yes | Yes | Yes |
| Scale invariant | Yes (pyramid) | Yes | Yes | Yes |
| Patent-free | Yes | No (until 2020) | No | Yes |
ORB-SLAM2 extracts 1,000 features per frame by default, distributed across 8 pyramid levels with a scale factor of 1.2. That means level 0 holds the highest-resolution features and level 7 is 1.2⁷ ≈ 3.58× downsampled. The distribution is enforced with a quadtree: the image is recursively split into four quadrants until each cell contains at most one keypoint. This prevents all 1,000 features clustering on a high-texture poster.
The Three-Thread Architecture
ORB-SLAM splits work across three threads that run concurrently.
Tracking thread runs every frame. It localizes the current camera against the map. If tracking was lost, it runs relocalization using a bag-of-words database. It decides whether the current frame is a new keyframe. Budget: 15–30 ms per frame.
Local mapping thread processes new keyframes. It triangulates new map points, runs local bundle adjustment over the covisibility graph, and culls redundant keyframes and outlier points. Budget: variable, 50–500 ms per keyframe.
Loop closing thread runs on keyframe insertion. It queries the DBoW2 database for candidate matches, verifies with a similarity transform (Sim3), and if confirmed, corrects the map and runs pose-graph optimization. Budget: 100–2000 ms when a loop fires.
The covisibility graph is the key data structure. Two keyframes are connected if they observe at least 15 common map points. Local BA optimizes only the keyframes within graph distance 1 of the current keyframe, plus their observed points. This keeps the optimization window at roughly 20–40 keyframes. Full BA over 2,000 keyframes is reserved for loop closure and runs in the background with a cap of 5 iterations.
Map Points and Reprojection
A map point is a 3D landmark with a mean position, a set of observing keyframes, and a representative ORB descriptor. When a new keyframe arrives, the tracking thread projects each map point into the image using the predicted pose. If it lands inside the frame, it searches a small window (±15 px) for the matching descriptor. A match contributes to the pose estimate via motion-only bundle adjustment.
The number 15 is not arbitrary. It comes from the expected pose uncertainty between consecutive keyframes. At 30 Hz and a 0.5 m/s robot, a keyframe fires roughly every 0.3 m. Pose prediction error over that interval stays under 10 px at VGA resolution for a calibrated camera. A ±15 px search window covers 3σ.
Mathematical Foundation
The core operation is reprojection error minimization. Given a 3D map point $\mathbf{X}_j \in \mathbb{R}^3$ and a camera pose $\mathbf{T}_i \in SE(3)$, the predicted pixel is:
$$ \mathbf{u}_{ij} = \pi(\mathbf{T}_i \mathbf{X}_j), \quad \pi(\mathbf{X}) = \begin{bmatrix} f_x \frac{X}{Z} + c_x \ f_y \frac{Y}{Z} + c_y \end{bmatrix} $$
- $\mathbf{T}_i$ is the 4×4 camera-to-world transform (inverse of world-to-camera, depending on convention).
- $\pi$ is the pinhole projection.
- $f_x, f_y$ are focal lengths in pixels.
- $c_x, c_y$ are the principal point.
- $Z$ is the depth of the point in camera frame.
The reprojection residual is $\mathbf{e}{ij} = \mathbf{u}{ij} - \mathbf{z}{ij}$, where $\mathbf{z}{ij}$ is the observed pixel. Bundle adjustment minimizes:
$$ \min_{{\mathbf{T}i}, {\mathbf{X}j}} \sum{(i,j) \in \mathcal{C}} \rho\left(|\mathbf{e}{ij}|^2_{\Sigma_{ij}}\right) $$
- $\mathcal{C}$ is the set of all (keyframe, map point) observations.
- $\Sigma_{ij}$ is the 2×2 covariance of the observation (typically set to $\sigma^2 I$ with $\sigma = 1$ px).
- $\rho$ is the robust Huber cost, threshold 5.991 (the 95% chi-squared value for 2 DoF).
Worked numeric example. Suppose $f_x = f_y = 500$, $c_x = 320$, $c_y = 240$. A map point sits at $\mathbf{X} = (1.0, 0.5, 5.0)$ in camera frame. Projection: $u = 500 \cdot 1.0/5.0 + 320 = 420$, $v = 500 \cdot 0.5/5.0 + 240 = 290$. The observed pixel is $(422, 288)$. Residual is $(2, -2)$, squared norm 8. Since 8 > 5.991, the Huber cost kicks in and downweights this observation. It is not rejected outright, but its Jacobian contribution is scaled by $\sqrt{5.991/8} \approx 0.865$.
The Jacobian of the residual with respect to the point position is:
$$ \frac{\partial \mathbf{e}}{\partial \mathbf{X}} = -\begin{bmatrix} f_x/Z & 0 & -f_x X/Z^2 \ 0 & f_y/Z & -f_y Y/Z^2 \end{bmatrix} \mathbf{R} $$
where $\mathbf{R}$ is the rotation block of $\mathbf{T}_i$. In our example, $f_x/Z = 100$, $f_x X/Z^2 = 20$, $f_y/Z = 100$, $f_y Y/Z^2 = 10$. A 1 mm error in $X$ produces a 0.1 px residual. A 1 mm error in $Z$ produces 0.02 px. Depth is far less observable than lateral position, which is why monocular SLAM struggles with scale.
Practical Example
Consider a differential-drive warehouse AMR. Payload 40 kg, top speed 1.5 m/s, wheelbase 0.6 m. It carries a front-facing RGB-D camera (Intel RealSense D435i) at 0.4 m height, tilted 10° down. The camera runs at 30 Hz, 640×480, with depth range 0.3–3.0 m. The robot must stay within ±30 mm of the aisle centerline for the pallet forks to engage.
Step 1: Feature budget. At 1.5 m/s, the camera moves 50 mm between frames. ORB-SLAM extracts 1,000 features per frame. In a warehouse aisle with cardboard boxes, expect 600–800 to be valid (some fall on textureless floor). That is 18,000–24,000 features per second. Hamming matching against a 5,000-point local map takes roughly 6 ms on the D435i’s companion compute (a 4-core Atom).
Step 2: Depth fusion. RGB-D gives metric scale directly. Each ORB feature on the color image is back-projected using the aligned depth pixel. If depth is invalid (out of range or zero), the feature is used only for tracking, not for new map points. Expect 40% of features to have valid depth in a cluttered aisle. That is 300–400 new candidate points per keyframe.
Step 3: Keyframe selection. ORB-SLAM inserts a keyframe when tracking is weak (fewer than 50 matched points) or when the frame has moved more than a threshold. With a 0.3 m threshold, keyframes fire every 0.2 s at top speed. Over a 50 m aisle run, that is 167 keyframes. Local BA runs on each, touching 20–40 keyframes. Wall-clock cost is roughly 80 ms per keyframe on the Atom, which fits inside the 200 ms keyframe interval.
Step 4: Loop closure. The robot patrols a loop, returning to the start after 4 minutes. The loop closing thread queries DBoW2 with the current keyframe’s bag-of-words vector. A candidate is confirmed if the Sim3 alignment yields at least 20 inlier matches and the estimated scale is within 10% of 1.0 (RGB-D gives metric scale, so scale should be near unity). On confirmation, pose-graph optimization distributes the accumulated 4–6 m drift across 400+ keyframes. The corrected map is within 20 mm of the surveyed ground truth.
Step 5: Tolerance check. The ±30 mm aisle tolerance requires pose error under roughly 15 mm lateral (half the tolerance, to leave margin for control lag). After loop closure, the map is good enough. Between loops, drift accumulates at 1–2% of distance. Over 50 m, that is 0.5–1.0 m of drift before correction. This is why the robot must re-observe known landmarks every 30–40 m, not just at loop closure.
Common Pitfalls
1. Tracking loss on textureless surfaces. Warehouse floors, white walls, and glossy packaging produce almost no ORB corners. When matched points drop below 50, ORB-SLAM declares tracking lost and enters relocalization. WHY: FAST-9 needs 9 contiguous pixels brighter or darker by 20 intensity levels. A uniform gray floor fails this everywhere. DETECT: log the matched-point count per frame; a steady decline precedes loss by 0.5–1.0 s. FIX: add artificial texture (fiducial markers, floor decals), fuse wheel odometry as a motion prior, or switch to a stereo rig that can triangulate from wider baselines.
2. Scale ambiguity in monocular mode. A monocular ORB-SLAM map has arbitrary scale. A 1 m corridor and a 10 m corridor produce identical images up to scale. WHY: the projection equation loses depth without a second view or a metric sensor. DETECT: the map’s bounding box will not match surveyed dimensions; loop closure Sim3 will estimate a scale factor far from 1.0. FIX: use stereo or RGB-D, or initialize scale from a known object (a 0.5 m fiducial) and lock it.
3. Keyframe starvation on slow motion. ORB-SLAM inserts keyframes based on motion thresholds. A robot that pauses for 30 seconds keeps the same keyframe. If the scene changes (a forklift parks in front of the camera), the map goes stale. When motion resumes, tracking fails because the map no longer matches. WHY: keyframe criteria are motion-based, not scene-change-based. DETECT: monitor time-since-last-keyframe; flag if it exceeds 5 s. FIX: add a scene-change trigger that forces a keyframe when the inlier ratio drops below 0.7.
4. Loop closure false positives. DBoW2 can match two visually similar but physically distinct places, like two identical aisles. A false loop closure corrupts the entire map. WHY: bag-of-words ignores geometry; identical shelving produces identical word histograms. DETECT: the Sim3 inlier count will be low (< 15) or the estimated scale will deviate from 1.0. FIX: ORB-SLAM already requires geometric verification, but tighten it: require 30 inliers and a scale within 5% for RGB-D. Also check that the candidate keyframe is not a covisibility neighbor.
5. Too many features on high-texture objects. A single poster can absorb 400 of the 1,000 feature budget, starving the rest of the image. Tracking becomes fragile when the poster leaves the frame. WHY: ORB’s quadtree distributes features by image area, not by information content. A dense region still gets its share. DETECT: visualize keypoint distribution; a single 100×100 px region holding > 15% of keypoints is a red flag. FIX: reduce the per-cell cap or add a spatial uniformity penalty during extraction.
Key Takeaways
- ORB features are the right default for CPU-bound visual SLAM: 32-byte binary descriptors, Hamming matching, ~10 ms extraction at VGA. Do not reach for SIFT unless accuracy dominates and compute is free.
- ORB-SLAM’s three-thread split (tracking, local mapping, loop closing) is the architectural template. Budget 15–30 ms per frame for tracking and 50–500 ms per keyframe for local BA.
- The covisibility graph with a 15-observation threshold keeps local BA tractable. Optimizing 20–40 keyframes per iteration, not the whole map, is what makes real-time operation possible.
- Monocular scale is unobservable. If your robot needs metric accuracy, use stereo or RGB-D, or anchor scale to a known fiducial.
- Loop closure is mandatory for long-run consistency. Drift of 1–2% of distance means a 200 m patrol accumulates 2–4 m of error without it. Geometric verification (Sim3, ≥20 inliers) is what prevents false positives from corrupting the map.
Tomorrow
We move from cameras to spinning lasers: LiDAR SLAM with LOAM and LeGO-LOAM, where edge and planar features replace ORB corners, and scan-to-scan matching runs at 10 Hz on 64-beam Velodyne data.