Lesson 2.1.5 — Visual SLAM and Odometry

Week 10

Prerequisite lessons: 2.1.1 (camera model and image formation), 2.1.2 (object detection and 6D pose estimation), 2.1.3 (visual servoing), 2.1.4 (point clouds and registration), 1.1.1 (forward kinematics), 1.1.3 (computed-torque control).

Prior results used in this lesson. From 2.1.1: the pinhole projection equation $\tilde{\mathbf{p}} = \frac{1}{Z} \mathbf{K} \mathbf{X}_C$ and the inverse mapping from a depth image to a metric point cloud. From 1.1.1: the forward-kinematics map $T_W^E(q)$ that converts joint angles to the end-effector pose. From 2.1.4: point-cloud registration as the problem of estimating a rigid transform from corresponding 3D points. Each is restated at the point of use.

What this lesson hands forward. A working model of visual odometry and visual SLAM as relative-pose estimators that can run on the eye-in-hand camera when the arm’s proprioception is unavailable, unreliable, or insufficiently precise. Lesson 2.3.3 (contact-rich manipulation) uses this model to maintain object-pose estimates during contact, when the arm’s forward kinematics alone cannot resolve the small motions caused by compliance. Phase 3 uses it to supply the spatial grounding that VLA policies require.

The running case in one sentence. The same 6-DOF arm carries the same eye-in-hand RGB-D camera ($K = [[500,0,320],[0,500,240],[0,0,1]]$, $640 \times 480$) over the same table, cube, bin, and divider; this lesson adds a temporal image stream and asks what the camera can infer about its own motion from that stream alone.

§L.0 — Lesson map, objectives, and misconceptions

Observable outcomes. By the end of this lesson you can, without notes: (1) derive the epipolar constraint and explain why the essential matrix has five degrees of freedom; (2) implement the eight-point algorithm and explain when the five-point algorithm is required; (3) describe ORB feature extraction and matching well enough to diagnose a failure; (4) use RANSAC to reject outlier correspondences and state the trade-off between iteration count and inlier ratio; (5) distinguish feature-based, direct, and semi-direct visual odometry by their error functions; (6) explain the role of keyframes and local bundle adjustment in a visual SLAM system; (7) describe bag-of-words loop closure and pose-graph optimization at the level of inputs and outputs; (8) explain why monocular VO suffers scale ambiguity and how an IMU or a known kinematic chain resolves it; (9) design a drift evaluation for the eye-in-hand camera against forward-kinematics ground truth.

Misconception inventory.

Everything is self-contained. Where a prerequisite lesson is used, the result is restated at the point of use.

§L.0.5 — Retrieval warm-up

Q1. In 2.1.1, a pixel $(u,v)$ and depth $Z$ map to a camera-frame point $\mathbf{X}_C$. Write the mapping and state what happens when $Z$ is unknown.

Answer. $\mathbf{X}_C = Z \, \mathbf{K}^{-1} [u,v,1]^\top$. If $Z$ is unknown, the back-projected ray is known but the point’s distance along that ray is not; monocular VO must estimate depth by triangulation across views. Used in §L.A.1 to explain why two views are the minimum for metric structure.

Q2. In 1.1.1, forward kinematics gives $T_W^E(q)$. What does this transform represent, and what frame does the eye-in-hand camera occupy relative to it?

Answer. $T_W^E(q)$ is the pose of the end-effector frame $E$ in the world frame $W$. The camera is rigidly mounted to the end-effector, so $T_E^C$ is constant and $T_W^C(q) = T_W^E(q) \, T_E^C$. Used in §L.E.2 as the kinematic prior for hand-eye fusion.

Q3. In 2.1.4, point-cloud registration estimates a rigid transform from corresponding 3D points. What is the minimum number of non-collinear correspondences for a unique solution, and what does the residual measure?

Answer. Three non-collinear correspondences suffice for a unique rigid transform in the absence of noise. The residual is the sum of squared distances between transformed source points and target points. Used in §L.C.2 to contrast direct VO’s photometric residual with geometric registration.

Q4. In 2.1.3, visual servoing drives a camera-to-target error to zero. Why does that controller not need a map?

Answer. Visual servoing closes the loop on the current image; it never integrates motion over time, so it does not accumulate drift. Used in §L.D.1 to explain what changes when the camera must estimate its own motion rather than servo to a target.

Part A — The visual odometry problem

§L.A.1 — Two views, one constraint

Visual odometry is the problem of estimating the camera’s motion from a sequence of images. The word odometry is deliberate: like wheel odometry, it integrates incremental motion and therefore drifts. Unlike wheel odometry, it uses no wheel.

Consider two calibrated views of a static scene, taken by the same camera at poses $T_1 = (R_1, \mathbf{t}_1)$ and $T_2 = (R_2, \mathbf{t}_2)$, with $T \in SE(3)$. A 3D point $\mathbf{X}_W$ projects to normalized image coordinates $\tilde{\mathbf{p}}_1$ and $\tilde{\mathbf{p}}_2$:

where $\pi(\mathbf{X}) = [X/Z, \, Y/Z, \, 1]^\top$ is the normalized projection. The camera-frame coordinates are related by the relative transform $T_{21} = T_2^{-1} T_1$:

The epipolar constraint eliminates the unknown 3D point. The vectors $\mathbf{X}_{C_1}$, $\mathbf{X}_{C_2}$, and $\mathbf{t}_{21}$ are coplanar, so their scalar triple product vanishes:

where $\widehat{\mathbf{t}}$ is the skew-symmetric matrix of $\mathbf{t}$. In normalized coordinates, $\mathbf{X}_{C} \propto \tilde{\mathbf{p}}$, so

The matrix $E$ is the essential matrix (Longuet-Higgins 1981, tier A, S). It encodes the relative rotation and the direction of translation between the two views. It does not encode the magnitude of $\mathbf{t}_{21}$: scaling $\mathbf{t}_{21}$ by any positive scalar leaves $E$ unchanged because $\widehat{\alpha \mathbf{t}} = \alpha \widehat{\mathbf{t}}$ and the constraint is homogeneous. This is the algebraic origin of scale ambiguity in monocular VO: two views determine five degrees of freedom (three rotation, two translation direction) out of the six in $SE(3)$.

Concept check. A colleague claims that because $E$ has nine entries, nine correspondences are needed. Name the mechanism that reduces the effective count to five.

Answer. $E$ is homogeneous (scale irrelevant) and satisfies two algebraic constraints: $\det(E)=0$ and $2 E E^\top E - \operatorname{tr}(E E^\top) E = 0$. Nine entries minus one scale minus two constraints leaves six, and the sign ambiguity of $\mathbf{t}$ leaves five observable degrees of freedom; the minimal solver uses five correspondences (Nistér 2004, tier A, S). [displaces M5]

Worked-example ladder.

WE-L.A.a (fully worked). Suppose a calibrated camera observes a point at normalized coordinates $\tilde{\mathbf{p}}_1 = [0.2, 0.1, 1]^\top$ and $\tilde{\mathbf{p}}_2 = [0.15, 0.12, 1]^\top$. Assume the true motion is a pure translation $\mathbf{t}_{21} = [0.1, 0, 0]^\top$ and $R_{21} = I$. Compute $E$ and verify the constraint.

Solution. $\widehat{\mathbf{t}}_{21} = [[0,0,0],[0,0,-0.1],[0,0.1,0]]$. Then $E = \widehat{\mathbf{t}}_{21}$. Compute $\tilde{\mathbf{p}}_2^\top E \tilde{\mathbf{p}}_1$. First $E \tilde{\mathbf{p}}_1 = [0, -0.1, 0.02]^\top$. Then $\tilde{\mathbf{p}}_2^\top [0, -0.1, 0.02]^\top = 0.15 \cdot 0 + 0.12 \cdot (-0.1) + 1 \cdot 0.02 = -0.012 + 0.02 = 0.008$. The residual is not zero because the chosen image points are not exactly consistent with the assumed motion; the constraint holds exactly only for noise-free correspondences generated by that motion. A real pipeline minimizes the algebraic residual over all correspondences.

WE-L.A.b (completion). Given $\tilde{\mathbf{p}}_1 = [0.1, -0.05, 1]^\top$, $\tilde{\mathbf{p}}_2 = [0.08, -0.04, 1]^\top$, and $R_{21} = R_z(0.01)$, find the direction of $\mathbf{t}_{21}$ that makes the epipolar residual zero. Step: form $E = \widehat{\mathbf{t}} R_{21}$, write the constraint as a linear equation in $\mathbf{t}$, and solve for the null space. Completion: the null space of the $1 \times 3$ system gives $\mathbf{t} \propto [\dots]$. The point of the exercise is that one correspondence constrains $\mathbf{t}$ to a plane; two correspondences intersect to a line; five determine it.

WE-L.A.c (faded). Derive the eight-point algorithm: stack the constraints $\tilde{\mathbf{p}}_2^{(i)\top} E \tilde{\mathbf{p}}_1^{(i)} = 0$ for $i=1,\dots,8$ into a matrix $A \in \mathbb{R}^{8 \times 9}$ and solve $A \mathbf{e} = 0$. Faded step: show that the solution is the right singular vector of $A$ corresponding to the smallest singular value, then project the resulting $E$ onto the essential manifold by clamping its singular values to $(s,s,0)$. The derivation is in Scaramuzza & Fraundorfer 2011 (tier A, S), and the projection is necessary because the linear solution does not satisfy $\det(E)=0$.

WE-L.A.d (contrastive near-miss). A learner applies the eight-point algorithm to points that are all coplanar (the cube’s top face). The linear system returns a rank-deficient $E$, and the decomposition produces a translation direction that is wrong by a large angle. Why it fails: the eight-point algorithm is degenerate for coplanar points (Scaramuzza & Fraundorfer 2011, tier A, S). The fix: use the five-point algorithm, which handles coplanar points because it enforces the essential-manifold constraints during solving (Nistér 2004, tier A, S). This near-miss is exactly the case in the running workspace, where the cube’s planar face occupies many correspondences.

Comparative table: motion-estimation solvers.

All four require a robust wrapper (RANSAC) when correspondences contain outliers. The table’s role is to tell you which solver to put inside the wrapper given the scene geometry.

Design principle P32.1. Estimate relative motion, never absolute motion, from a single pair of views. Absolute pose is an integral; treating a pairwise estimate as absolute injects the drift of every prior frame into the current estimate without the opportunity to correct it.

Consolidation checkpoint 1

(a) Three capabilities. You can write the epipolar constraint and identify its three ingredients ($E$, $\tilde{\mathbf{p}}_1$, $\tilde{\mathbf{p}}_2$). You can explain why $E$ has five degrees of freedom. You can name the degenerate case of the eight-point algorithm.

(b) Transfer probe. A stereo camera has a known baseline $b$. Does stereo VO suffer scale ambiguity? Answer: no, because the baseline provides the missing metric scale; triangulation from a known baseline yields metric depth.

(c) Remediation rule. If you cannot state why scale is unobservable from two monocular views, re-derive the homogeneity of the epipolar constraint before continuing.

Part B — Feature-based visual odometry

§L.B.1 — Detecting and describing features

A feature-based VO pipeline converts each image into a set of keypoints with descriptors, matches descriptors across frames, rejects outliers, and estimates $E$ or $T$ from the surviving correspondences.

The dominant choice in real-time robotics is ORB (Oriented FAST and Rotated BRIEF; Rublee et al. 2011, tier A, S). ORB combines the FAST corner detector with the BRIEF binary descriptor and adds an orientation estimate so that the descriptor is rotation-invariant. FAST is fast because it compares a circle of sixteen pixels around a candidate to the center intensity; if a contiguous arc of at least nine pixels is all brighter or all darker by a threshold, the center is a corner. BRIEF compares pairs of pixels within a patch and records the sign of each comparison as one bit; the resulting descriptor is a binary string, and matching is Hamming distance, which is a population count on XOR.

The running case’s eye-in-hand camera sees a tabletop with a cube, a bin, and a divider. The table surface is largely textureless; the cube’s edges and the divider’s corners provide the strongest FAST responses. A practical pipeline must therefore expect clustered features and must not assume uniform spatial coverage.

§L.B.2 — Matching and the outlier problem

Given descriptors in frame $k-1$ and frame $k$, matching proposes correspondences. The simplest rule is nearest neighbour in descriptor space. For binary descriptors, Hamming distance is cheap, but nearest-neighbour matching produces many false matches when the scene contains repeated structure (the table’s texture, the bin’s rim).

The standard filter is Lowe’s ratio test: retain a match only if the distance to the nearest neighbour is less than a fraction $\rho$ (typically 0.7–0.8) of the distance to the second-nearest neighbour. The ratio test removes ambiguous matches, but it does not remove all outliers. The residual outliers are handled by RANSAC (Fischler & Bolles 1981, tier A, S).

RANSAC for essential-matrix estimation repeats three steps: (i) sample the minimal number of correspondences (five for the five-point solver), (ii) fit $E$, (iii) count inliers whose epipolar residual is below a threshold. The number of iterations required for confidence $p$ with inlier ratio $w$ is

where $s$ is the sample size. With $s=5$, $w=0.5$, and $p=0.99$, $N \approx 146$. With $w=0.2$, $N \approx 57{,}000$: the iteration count explodes as the inlier ratio falls. This is why the ratio test matters — it raises $w$ before RANSAC pays the exponential penalty.

Concept check. A learner increases the RANSAC inlier threshold to make more matches count as inliers. What two quantities move in opposite directions, and which one should be monitored on a held-out sequence?

Answer. Raising the threshold increases the inlier count $w$ (reducing iterations) but admits geometrically inconsistent matches, biasing the estimated $E$. Monitor the reprojection residual on a held-out set of matches, not the inlier count. [displaces M2]

Worked-example ladder.

WE-L.B.a (fully worked). Suppose a frame pair yields 500 putative matches. After the ratio test, 200 survive. RANSAC with the five-point solver and $p=0.99$ is run. If the true inlier ratio among the 200 is $w=0.4$, how many iterations are needed, and what is the probability that at least one all-inlier sample occurs in 300 iterations?

Solution. $N = \log(0.01) / \log(1 - 0.4^5) = \log(0.01)/\log(1 - 0.01024) \approx 4.605 / 0.01029 \approx 447$. So 300 iterations is insufficient for 99% confidence; the probability of at least one all-inlier sample in 300 iterations is $1 - (1 - 0.4^5)^{300} = 1 - (0.98976)^{300} \approx 1 - 0.046 = 0.954$. A pipeline that hard-codes 300 iterations will fail on this pair roughly 4.6% of the time.

WE-L.B.b (completion). For the same pair, compute the number of iterations if the ratio test is tightened so that $w$ rises to 0.6. Completion: $0.6^5 = 0.07776$, $N = 4.605 / 0.0809 \approx 57$. Tightening the ratio test from 0.8 to 0.7 can cut RANSAC iterations by an order of magnitude. The cost is fewer surviving matches, which may hurt triangulation.

WE-L.B.c (faded). Implement the five-point solver’s null-space step: given five correspondences, build the $5 \times 9$ matrix $A$, compute its SVD, and take the four-dimensional null space. The essential matrix is a linear combination $E = x E_1 + y E_2 + z E_3 + E_4$ of the null-space basis; the coefficients solve the cubic constraints. Faded step: write the two constraints $\det(E)=0$ and $2 E E^\top E - \operatorname{tr}(E E^\top) E = 0$ as a polynomial system in $(x,y,z)$ and solve by resultant or companion matrix (Nistér 2004, tier A, S).

WE-L.B.d (contrastive near-miss). A learner matches features between the eye-in-hand image and a database image of the same scene taken from a different viewpoint, then estimates $E$ and decomposes it as if the two views were consecutive. The translation direction is plausible, but the scale is arbitrary and the rotation is correct. Why it fails: $E$ decomposition returns $(R, \mathbf{t}/|\mathbf{t}|)$; a non-consecutive pair has no known baseline, so the translation magnitude is meaningless. The fix: for relocalization against a map, use 3D-to-2D correspondences and PnP, not 2D-to-2D $E$. The lesson is that the type of correspondence determines the estimator.

Comparative table: feature detectors in the running case.

The table’s role is diagnostic: if the pipeline reports fewer than 100 matches, the detector is not the problem; the scene lacks texture or the exposure is wrong.

Design principle P32.2. Raise the inlier ratio before raising the RANSAC iteration budget. The ratio test is cheaper than RANSAC iterations and improves the estimate as well as the speed.

§L.B.3 — From matches to motion and structure

Once $E$ is estimated and decomposed into $R_{21}$ and $\mathbf{t}_{21}/|\mathbf{t}_{21}|$, the pipeline triangulates the matched points to obtain 3D structure. Triangulation solves, for each correspondence, the least-squares problem

The linear triangulation method stacks the two projection equations and solves a $4 \times 4$ homogeneous system; the nonlinear refinement minimizes reprojection error and is the local step of bundle adjustment.

Triangulation quality depends on parallax. If the camera translates along the optical axis (the running case’s arm moving straight toward the cube), the epipolar geometry is near-degenerate: the baseline is small relative to depth, and depth uncertainty is large. A feature-based VO pipeline should reject triangulated points whose depth uncertainty exceeds a threshold, or whose triangulation angle is below a few degrees. This is the geometric reason why a purely forward-moving arm produces poor VO.

Concept check. The arm moves the camera from $x=0.6$ to $x=0.62$ while looking at the cube at $x=0.5$. Is the parallax large or small, and what does that imply for triangulated depth?

Answer. The baseline is 0.02 m at a depth of roughly 0.1–0.3 m; the triangulation angle is small, so depth is poorly conditioned. VO should weight rotation heavily and treat translation/depth as low-confidence until a larger baseline accumulates. [displaces M2]

Consolidation checkpoint 2

(a) Three capabilities. You can compute the RANSAC iteration count from an inlier ratio. You can explain the ratio test’s effect on $w$. You can state the parallax condition under which triangulation is unreliable.

(b) Transfer probe. A stereo rig with baseline 0.1 m observes a point at 2 m. What is the triangulation angle, and is depth well conditioned? Answer: the angle is roughly $\arctan(0.1/2) \approx 2.9^\circ$; depth is marginal, and a larger baseline or longer focal length would improve it.

(c) Remediation rule. If you cannot derive the RANSAC iteration formula, re-derive it from the probability of at least one all-inlier sample before proceeding to Part C.

Part C — Direct and semi-direct visual odometry

§L.C.1 — The photometric error

Feature-based VO throws away most of the image: it uses only the pixels around detected corners. Direct methods use the image intensities themselves and minimize a photometric error. For a point $\mathbf{X}$ visible in both frames, direct VO assumes that the intensity of its projection is constant:

The residual is the intensity difference, and the optimization is over the relative pose (and often the depths of the sampled points). The advantage is that direct methods exploit gradients everywhere, including on edges and weak corners that FAST would not detect. The disadvantage is sensitivity to illumination change, exposure, and rolling shutter; the photometric-consistency assumption is stronger than the geometric-consistency assumption of feature matching.

DSO (Direct Sparse Odometry; Engel et al. 2018, tier A, S) is the canonical sparse direct method. It samples points with high intensity gradients, maintains a sliding window of keyframes, and jointly optimizes camera poses, point depths, and camera intrinsics/photometric parameters by minimizing a robustified photometric error. DSO does not detect loops and is therefore odometry, not SLAM: it drifts.

§L.C.2 — Semi-direct: SVO

SVO (Semi-direct Visual Odometry; Forster et al. 2017, tier A, S) combines the two paradigms. It uses direct alignment to track pixels that have high gradients between frames, but it uses feature-based bundle adjustment to refine structure and motion. The direct step is fast because it avoids descriptor extraction and matching; the feature-based step is accurate because it enforces geometric consistency. SVO is designed for downward-looking cameras on micro aerial vehicles, but its architecture is instructive: the choice between direct and feature-based is not binary; it is a choice of which subproblem each method handles best.

Worked-example ladder.

WE-L.C.a (fully worked). For a one-dimensional image row with intensities $I_1 = [0, 0.2, 0.8, 0.2, 0]$ and a shifted frame $I_2 = [0, 0.2, 0.8, 0.2, 0]$ (no motion), compute the photometric residual for a point at pixel 2 under a hypothesized shift of 1 pixel. Solution: the residual is $I_1(2) - I_2(3) = 0.8 - 0.2 = 0.6$. Under zero shift the residual is $0.8 - 0.8 = 0$. The optimizer therefore prefers zero shift. Now add a brightness offset of $+0.1$ to $I_2$: the zero-shift residual becomes $-0.1$, and the optimizer may prefer a nonzero shift to compensate. This is the failure mode that affine brightness parameters in DSO are designed to absorb.

WE-L.C.b (completion). Derive the Gauss-Newton update for a single point’s depth under photometric error. Completion: linearize $I_2(\pi(T \mathbf{X}(d)))$ with respect to $d$, form the scalar normal equation, and update $d \leftarrow d - J^\top r / (J^\top J + \lambda)$. The damping $\lambda$ (Levenberg–Marquardt) is necessary because the photometric error is non-convex.

WE-L.C.c (faded). Compare the computational cost of SVO’s direct tracking step to ORB’s descriptor matching for a 640×480 frame with 500 points. Faded step: SVO’s direct step evaluates intensities at projected locations (constant cost per point); ORB extracts descriptors (cost per keypoint plus patch comparisons) and matches (cost per candidate pair). The ratio depends on implementation, but the asymptotic difference is that matching grows with the number of candidate pairs while direct tracking grows linearly in points.

WE-L.C.d (contrastive near-miss). A learner runs DSO on the eye-in-hand camera while the arm’s lighting is switched from fluorescent to LED. The photometric residuals jump, and the estimated trajectory develops a large rotation error. Why it fails: DSO’s brightness-constancy assumption is violated; the affine brightness model is insufficient for a spectral change. The fix: either use feature-based VO (which is robust to illumination because descriptors are based on gradient ordering, not absolute intensity), or add exposure compensation and re-calibrate the photometric parameters. The near-miss shows that direct methods trade illumination robustness for texture exploitation.

Comparative table: VO paradigms.

The table’s role is selection: for the running case’s tabletop, with controlled lighting and a mix of edges and texture, feature-based is the safe default; direct methods are preferable when the scene is texture-poor but photometrically stable.

Design principle P32.3. Match the error function to the invariant you trust. If you trust geometry more than intensity, use reprojection error; if you trust intensity more than corner detectability, use photometric error; if you trust neither alone, use both (SVO).

Consolidation checkpoint 3

(a) Three capabilities. You can state the photometric-consistency assumption and its two main violations. You can explain SVO’s division of labour. You can choose a VO paradigm for a given scene.

(b) Transfer probe. A warehouse robot with flickering fluorescent lights must run VO. Which paradigm, and why? Answer: feature-based, because binary descriptors are invariant to monotonic intensity changes; direct methods would need explicit photometric calibration that flicker defeats.

(c) Remediation rule. If you cannot name the error function of each paradigm, do not proceed to Part D; the rest of the lesson assumes you can.

Part D — Visual SLAM

§L.D.1 — Why odometry is not enough

Odometry integrates relative motion. Every relative estimate carries noise, and integration accumulates it. Over a 10-second trajectory with 1% translation error per frame at 30 Hz, the position error grows without bound. SLAM (Simultaneous Localization and Mapping) adds two mechanisms that bound the drift: mapping (maintaining a consistent set of landmarks) and loop closure (recognizing a previously visited place and correcting the trajectory).

The word “accumulates” hides a shape, and the shape decides what a fix can do. Integration of independent per-frame errors gives a random walk: the position error’s variance grows linearly in the number of frames, and its direction is strongly correlated from frame to frame. The lab measures lag-1 autocorrelation of the error vector at $+0.995$. A random walk’s RMS is dominated by the middle of the trajectory, not its end — which is why a single loop closure can cut the endpoint error by 63% while leaving the global RMSE 12.6% higher. Bounding a random walk needs constraints spread through it, not one constraint at its end.

The distinction between VO and SLAM is not the front end; it is the back end. A VO system has no persistent map and no loop closure. A SLAM system has both.

§L.D.2 — Keyframes and local mapping

A visual SLAM system cannot bundle-adjust every frame at 30 Hz. It selects keyframes: frames that are sufficiently different from the last keyframe (by translation, rotation, or inlier count) to justify a new map update. Non-keyframes are tracked relative to the current keyframe but do not enter the map.

Local bundle adjustment optimizes the poses of the most recent keyframes and the 3D points they observe, while holding older keyframes fixed. The factor graph is sparse: each point is observed by a small subset of keyframes, so the Hessian has a block-arrow structure that sparse solvers exploit. This is the mechanism that lets BA scale to thousands of points; a dense EKF over the same state cannot, because its covariance is quadratic in the state dimension (see Misconception 3).

§L.D.3 — Loop closure and pose-graph optimization

Loop closure has two stages: detection and correction.

Detection answers the question “have I been here before?” The standard approach is appearance-based bag-of-words place recognition. Visual words are clustered descriptors; an image is represented as a histogram of word occurrences. A database of previous images is queried with the current image’s histogram, and candidates are ranked by similarity. DBoW2 (Gálvez-López & Tardós 2012, tier A, S) is the implementation used in ORB-SLAM and VINS-Mono. Detection is followed by geometric verification: the candidate pair must yield enough inlier feature matches under RANSAC, otherwise it is discarded. Without geometric verification, perceptual aliasing (two similar-looking corridors) produces false loops, which corrupt the map.

Correction adds the loop constraint to a pose graph and optimizes. The nodes are keyframe poses; the edges are relative-pose constraints from odometry and from loop closures. The optimization distributes the loop-closure error across the trajectory. In a 4-DOF pose graph (as in VINS-Mono), roll and pitch are held fixed because they are observable from the IMU; the optimization is over $x, y, z, \text{yaw}$. In a full 6-DOF pose graph, all six are optimized.

Concept check. A loop closure is detected with high appearance similarity but only three geometric inliers. Should it be accepted? Name the mechanism that decides.

Answer. No. Geometric verification requires a minimum inlier count consistent with the pose-graph edge’s noise model; three inliers cannot constrain a 6-DOF relative pose. The mechanism is RANSAC-based verification with a threshold on inlier count, not appearance score alone. [displaces M4]

Worked-example ladder.

WE-L.D.a (fully worked). A pose graph has four nodes in a square: $x_0 \to x_1 \to x_2 \to x_3 \to x_0$. Odometry edges estimate each side as 1.0 m, but the loop edge estimates the closing side as 0.9 m. Set up the least-squares problem that distributes the 0.1 m error. Solution: parameterize each node’s position as a scalar along the loop, write four residual equations $x_{i+1} - x_i - 1.0 = 0$ for $i=0,1,2$ and $x_0 - x_3 - 0.9 = 0$, and solve the linear system. The solution shifts each node by 0.025 m, distributing the error equally. The worked example shows that pose-graph optimization is a linear least-squares problem when the constraints are scalar positions; with rotations it becomes nonlinear on $SE(3)$.

WE-L.D.b (completion). Add a second loop closure between $x_1$ and $x_3$ with a measured distance of 1.4 m. Completion: the system is now overdetermined (five edges, four nodes), and the least-squares solution minimizes the sum of squared residuals. The point is that loop closures add constraints, not equations; the optimizer reconciles inconsistent constraints.

WE-L.D.c (faded). Implement DBoW2 scoring: given a vocabulary of $k$ words and two histograms $\mathbf{h}_1, \mathbf{h}_2$, compute the $L_1$ similarity $s = 1 - \frac{1}{2}|\mathbf{h}_1/|\mathbf{h}_1|_1 - \mathbf{h}_2/|\mathbf{h}_2|_1|_1$. Faded step: normalize histograms, compute the $L_1$ distance, and threshold. The scoring is cheap because histograms are sparse.

WE-L.D.d (contrastive near-miss). A learner adds every loop-closure candidate with appearance similarity above 0.8 to the pose graph without geometric verification. The map folds onto itself: distinct parts of the table are merged because the tabletop texture is repetitive. Why it fails: perceptual aliasing; the appearance model cannot distinguish two similar patches. The fix: geometric verification plus a consistency check (the loop constraint must be consistent with the odometry chain within a Mahalanobis bound). This is the failure that the geometric-verification stage exists to prevent.

Comparative table: SLAM back ends.

The table’s role is architectural: the running case needs poses, not a dense map, so a pose-graph back end with keyframe BA for local accuracy is the right fit.

Design principle P32.4. Treat loop closure as a hypothesis, not a measurement. Appearance proposes; geometry disposes; consistency confirms.

Consolidation checkpoint 4

(a) Three capabilities. You can explain the role of keyframes. You can describe bag-of-words detection and geometric verification. You can set up a scalar pose-graph optimization.

(b) Transfer probe. A robot revisits a room after 30 minutes; the lighting has changed. Which loop-closure stage is most likely to fail, and what is the mitigation? Answer: appearance-based detection is most likely to fail because the histogram changes; the mitigation is sequence-based or feature-descriptor-based matching that is robust to illumination, plus geometric verification.

(c) Remediation rule. If you cannot explain why geometric verification is necessary after appearance matching, re-read §L.D.3 before the lab.

Part E — Visual-inertial odometry and the running case

§L.E.1 — Scale ambiguity and its resolutions

Monocular VO estimates translation up to scale. Three mechanisms resolve the scale in practice: (i) a known baseline (stereo or RGB-D), (ii) an IMU, (iii) a known object or known motion.

Scale error and shape error are different failures and they drift differently. A monocular estimator’s scale error is set by whatever fixed the factor once — a baseline, an IMU’s first excitation, an assumed first step — and it does not accumulate afterwards. The lab measures this: with the first step assumed at 10 mm the final scale error is 2.85%, and sweeping that single assumption to 5 mm, 7.5 mm, 15 mm and 20 mm moves the error to 51.43%, 27.14%, 45.72% and 94.30%. Path length does not enter. What does accumulate is the shape error, and it is what makes monocular ATE (0.1593 m) 3.6× worse than RGB-D’s (0.0438 m) even at matched scale.

The running case has an RGB-D camera. Depth from the sensor provides metric scale directly, so the eye-in-hand camera is not scale-ambiguous when the depth channel is valid. But the depth channel fails on shiny or transparent surfaces and at grazing angles; during those intervals the pipeline falls back to monocular VO, and scale becomes ambiguous again. The arm’s forward kinematics provides an independent metric prior: the camera’s motion is constrained by $T_W^C(q) = T_W^E(q) T_E^C$. This is the kinematic prior that makes the running case’s VO more robust than a free-floating monocular camera.

An IMU provides acceleration and angular velocity. Visual-inertial odometry fuses the two. The standard formulation uses IMU preintegration (Forster et al. 2017, tier A, S): IMU measurements between keyframes are integrated into a single relative-motion constraint that is independent of the linearization point and carries bias corrections. VINS-Mono (Qin et al. 2018, tier A, S) is a tightly-coupled monocular VIO that performs sliding-window nonlinear optimization over visual and inertial factors, with online initialization, loop detection, and 4-DOF pose-graph optimization. Its architecture is the reference for any system that must combine a camera with an IMU.

Concept check. The arm holds the camera stationary for two seconds. The IMU reads zero acceleration (after bias removal), and the camera sees no optical flow. Is scale observable during this interval?

Answer. No. VINS-Mono requires acceleration excitation to make scale observable (Qin et al. 2018, tier A, S); a stationary period provides no excitation, so the estimator must coast on the prior or declare the scale unobservable. [displaces M5]

§L.E.2 — The running case: hand-eye fusion

The eye-in-hand camera is rigidly attached to the end-effector. The transform $T_E^C$ can be calibrated once (hand-eye calibration) and treated as constant. During operation, the arm reports $q$, and forward kinematics gives $T_W^E(q)$. The camera’s pose is therefore known up to the accuracy of the kinematic model and the calibration.

Visual odometry provides an independent estimate $T_{k-1}^k$ from images. The two estimates can be fused:

A simple fusion is a complementary filter: trust the kinematics for low-frequency accuracy and the VO for high-frequency motion, with a blending gain. A principled fusion treats both as factors in a graph and optimizes; that is the VIO formulation with the kinematic chain as an additional factor.

The blending gain is where a fixed weight fails, and the lab measures the failure. A 50/50 blend is worse than the kinematic prior alone in every configuration tested — 0.0221 m against 0.0048 m when the prior is accurate to 3 mm, and still 0.0192 m against 0.0282 m when it is biased by 40 mm. Inverse-variance weighting wins in nine of ten configurations, and ties in the tenth, where the prior is unbiased and ten times more accurate than VO. Fusion helps in proportion to how much the worse source has to contribute, and the weight is the only thing that sets that proportion.

The running case’s lab uses the kinematic estimate as ground truth to evaluate VO drift, not to replace it. This is legitimate because the kinematic ground truth is independent of the image stream; it is not a circular evaluation.

There is a subtler circularity to watch for, and the lab has to avoid it. If the kinematic prior used in the fusion arm were generated as the same trajectory the ATE is scored against, then fusion would be scored against its own input, and H4 would confirm itself. The lab generates the prior by adding a bias plus Gaussian noise to the true trajectory at four bias levels and two noise levels, and reports the fusion result against the truth for each. The prior is a corrupted copy of the truth, which is what a real FK chain is; it is not the truth. The distinction matters because it is what lets the fusion table show the prior losing to VO at 40 mm bias.

Worked-example ladder.

WE-L.E.a (fully worked). Suppose the arm moves the camera by $0.05$ m in $x$. Forward kinematics reports the move with 0.5 mm accuracy. VO on the RGB-D stream estimates the move as $0.048$ m. Compute the VO scale error and the fused estimate under a 50/50 blend, then under inverse-variance weights. Solution: scale error $= (0.048 - 0.05)/0.05 = -4$. The 50/50 estimate is $0.049$ m, whose error is 1 mm — twice the 0.5 mm the kinematic prior already had. With $\sigma_{\text{kin}} = 0.5$ mm and $\sigma_{\text{VO}}$ set by its own error, the inverse-variance weight on VO is small and the fused error stays near 0.5 mm. The worked example shows that a fixed blend can degrade the better source; the weights must reflect the relative uncertainties. The lab measures exactly this: the 50/50 blend is worse than kinematics alone in every configuration.

WE-L.E.b (completion). Derive the covariance-weighted fusion of two scalar estimates $a$ with variance $\sigma_a^2$ and $b$ with variance $\sigma_b^2$. Completion: $x = (\sigma_b^2 a + \sigma_a^2 b)/(\sigma_a^2 + \sigma_b^2)$, with variance $\sigma_a^2 \sigma_b^2/(\sigma_a^2+\sigma_b^2)$. The fusion weight is the inverse-variance weight, not a fixed blend.

WE-L.E.c (faded). Write the IMU preintegration equations for position, velocity, and orientation between two keyframes, including bias Jacobians. Faded step: the position preintegrated term is $\Delta \mathbf{p}_{ij} = \sum_k \Delta \mathbf{v}_k \Delta t + \frac{1}{2} \Delta R_k \tilde{\mathbf{a}}_k \Delta t^2$; include the bias correction $\partial \Delta \mathbf{p}/\partial \mathbf{b}_a$ and the gravity term. The full derivation is in Forster et al. 2017 (tier A, S).

WE-L.E.d (contrastive near-miss). A learner fuses VO and kinematics by averaging the two quaternions component-wise. The resulting rotation is not a unit quaternion, and the camera pose drifts in roll. Why it fails: quaternion averaging is not closed under addition; the average of two unit quaternions is not a unit quaternion. The fix: use spherical linear interpolation (slerp) or, better, optimize on the manifold with a local tangent-space parameterization. This is the same lesson as 0.2.2 (rotation representations) applied to sensor fusion.

Comparative table: scale sources in the running case.

The table’s role is redundancy design: no single source is always available, so the pipeline should declare which source is active and degrade gracefully.

Design principle P32.5. Fuse on the manifold, not in coordinates. Rotations, poses, and directions live on manifolds; adding them as vectors produces artefacts that look like drift but are algebra errors.

Consolidation checkpoint 5

(a) Three capabilities. You can explain the three scale sources. You can write the kinematic prior for the eye-in-hand camera. You can state the inverse-variance fusion rule.

(b) Transfer probe. The cube is temporarily occluded and the RGB-D depth returns zeros for 10 frames. What should the VO pipeline do? Answer: fall back to monocular VO, propagate with the kinematic prior, and inflate the pose covariance until depth returns; do not assume the last depth values are valid.

(c) Remediation rule. If you cannot state why a stationary IMU leaves scale unobservable, re-read §L.E.1 before the lab.

Failure taxonomy

The table covers the three lab corruptions plus three failures not exercised in the lab: rolling shutter, specular depth failure, and IMU bias drift.

The lab

Task. Build a feature-based visual odometry pipeline on an RGB-D sequence captured by the eye-in-hand camera, integrate the relative poses, compare the integrated trajectory against forward-kinematics ground truth, then add loop closure and a pose-graph correction. Every number quoted in the prose below was produced by the code that follows it.

Conditions. Python 3.10+, PyTorch 2.x (CPU), OpenCV for ORB and matching, NumPy. The sequence is synthetic: a yaw sweep out and back over 120 frames at 30 Hz, so the last frame revisits the first and a loop constraint genuinely exists. Landmarks are textured patches rather than bare points, because ORB needs corners and a point cloud renders to a blank image. Seed every RNG.

Controls. (i) ratio test at 0.75 plus RANSAC; (ii) no ratio test; (iii) monocular, no depth at all; (iv) VO fused with a noisy kinematic prior.

Metrics.

Two of the five targets are missed and one is missed by a factor of four. That is the point of running the lab: the targets in a syllabus are hypotheses too.

The canaries, and what they caught

The lab prints four canaries before any experiment. They are worth reading carefully, because the first version of this code passed one of them while being wrong.

C1 sends PnP from the identity to a far pose ($0.18, -0.12, 0.09$ rad, $0.22$ m) on exact pixels and recovers it to $1.1\times10^{-16}$. C2 repeats it with one Jacobian entry negated. The correct Jacobian converges in 6 iterations; the negated one runs all 60 and lands a full radian away.

C2 exists because C1 alone cannot see the Jacobian’s shape. With a zero residual the answer is a fixed point for any $J$, and a mildly wrong $J$ still walks there — a negated entry in the $v$ row converges in 7 iterations and lands within $3\times10^{-14}$. What a wrong $J$ costs is the basin of convergence. A gate on the exact answer alone passed a wrong Jacobian in this lab’s first draft.

C3 confirms the relative transform maps previous-camera points into the current camera frame, residual $2.2\times10^{-16}$. C4 confirms the chain is $T_k = T_{k-1}\,\mathrm{inv}(\Delta T)$ and that the plausible-looking $\Delta T\,T_{k-1}$ is wrong by $8.1\times10^{-2}$. Both directions converge on a short sequence; they differ on a long one, and only one of them is odometry.

Pre-registered hypotheses

H1 — the ratio test helps, and by more than advertised. Claim: the ratio test reduces mean relative translation error by ≥ 30%. Measured: 13.02% with it, 197.35% without — a 93.4% reduction. Supported, and the magnitude is understated. The mechanism is visible in the inlier counts: without the ratio test RANSAC is handed 671 matches of which it keeps 74; with the test it is handed 112 and keeps 40. The inlier ratio is 11.0% versus 35.7%, and RANSAC’s iteration budget depends on that ratio cubed — 3458 iterations versus 99 for 99% confidence.

H2 — monocular drift grows with path length while RGB-D’s does not. Claim: monocular scale error grows with path length while RGB-D’s does not. Measured: refuted on both halves. Monocular scale error is 11.27% over 20 frames, 0.75% over 40, 2.47% over 60, 2.87% over 120 — it does not grow, it settles. RGB-D’s is 10.70%, 7.64%, 7.90%, 5.64% — it falls.

The ATE tells the same story, and rules out the reading that the claim was about shape rather than scale:

Neither estimator’s ATE grows linearly with path length. RGB-D’s grows sub-linearly and monocular’s saturates and then falls. The ratio between them is roughly constant at 4–6×, which is the actual relationship: monocular VO is worse by a factor, not by a growth rate.

The refutation has a clean mechanism. Monocular VO recovers translation only up to a scale factor; the lab fixes that factor by assuming the first step is 10 mm. The 10 mm is a guess, and the final scale error is that guess’s error, not accumulated drift. Sweeping the prior shows it directly:

The scale error is a monotone function of the prior, and it is not a function of path length. Monocular VO’s ATE (0.1593 m) is still 3.6× RGB-D’s (0.0438 m), but for a different reason: the essential-matrix estimate is noisier per frame, and that noise random-walks. The hypothesis conflated two failures. A scale-free estimator’s scale error is its initialisation’s error; only the shape error drifts.

H3 — loop closure reduces final ATE by ≥ 50%. Claim: ≥ 50% reduction. Measured: refuted, and with the sign reversed: ATE goes from 34.66 mm to 39.04 mm, a 12.6% increase.

This is the lab’s most instructive refutation, because the loop closure works. The error at the loop frame falls from 40.25 mm to 14.82 mm — a 63% reduction, exactly the promised effect, at the node the constraint touches. The global RMSE rises because a loop edge is one absolute constraint on one node, and the error the chain accumulated in the middle is untouched by it.

The error here is a random walk (lag-1 autocorrelation of the error vector: +0.995). A random walk’s ATE is dominated by its middle, not its end. Correcting the end redistributes the residual into the middle and can raise the RMS. The measured behaviour under loop-edge noise confirms the mechanism: at 2 mm noise the ATE is 39.04 mm, at 20 mm it is 42.27 mm — the loop edge barely matters either way.

The lesson’s original claim assumed drift is monotone. It is not; it is a random walk, and a single loop closure bounds the endpoint, not the path. What actually reduces global ATE is a loop closure plus enough of them to constrain the interior — which is why real systems keep a pose graph and not a single edge.

H4 — inverse-variance fusion beats either source. Claim: fusion beats both. Measured: supported, but not for the stated reason, and not unconditionally.

Two things here contradict the textbook framing. First, the 50/50 blend is worse than kinematics alone in every row — it is a fixed weight, and a fixed weight cannot be right for both a 3 mm and a 12 mm prior. Second, honest inverse-variance weighting wins in nine of ten rows but ties when the prior is both unbiased and ten times more accurate than VO. Fusion beats the better source only when the better source is not already better by more than the fusion can recover; a weighted average of a good estimate with a bad one is a good estimate only if the weight is nearly zero.

The stated mechanism — “the two error sources are partially independent” — is measurable and holds: the correlation between $|$VO error$|$ and $|$kinematic error$|$ is $-0.029$. Independence is necessary for fusion to help. It is not sufficient; the weights carry the rest.

Break-it sequence

1. Corrupt 30% of the matches (silent). Measured: 1499 of 1614 injected outliers rejected — 92.9%, short of the 95% acceptance bar — and the ATE barely moves (0.0400 m against 0.0438 m clean). The corruption is genuinely silent. RANSAC absorbs it, the trajectory looks fine, and the only evidence is the inlier count falling from 40 to 28. This is the case a code review misses: the output is not wrong enough to notice.

2. Remove the ratio test (silent, loud). Measured: ATE 0.6074 m against 0.0438 m, a factor of 13.9. Unlike break-it 1 this one is not silent — the error is too large to overlook. It is still worth running, because it shows the ratio test doing work that RANSAC cannot pay for. RANSAC is handed 671 matches and keeps 74, an 11.0% inlier ratio; the 3-point budget for 99% confidence at that ratio is $\ln(0.01)/\ln(1-0.11^3) = 3458$ iterations. With the ratio test the ratio is 35.7% and the budget is 99. This lab runs 60 iterations in both arms, so the no-ratio-test arm is running roughly 1.7% of the budget its inlier ratio demands — it succeeds on luck, and the ATE shows the cost.

3. Freeze the depth channel (silent). Measured: ATE 0.1437 m against 0.0438 m, a 3.3× degradation over ten frames. The freeze is detectable in one frame: depth variance is $7.18\times10^{-2}$ live and $4.93\times10^{-32}$ frozen. A frozen channel has variance exactly zero, and that is a one-line check — which is why a pipeline that does not make it will integrate ten frames of nonsense without complaint.

Acceptance criteria

What good looks like. (1) The canaries are printed before the experiment, and at least one of them is a negative canary — C2’s negated Jacobian — because a gate that only ever sees correct code cannot tell you it works. (2) Every hypothesis is reported with the number that decides it, including the ones that fail; the three refutations here are more informative than the four confirmations would have been. (3) The scale-prior sweep is reported beside the scale error, because a scale error without its prior is uninterpretable. (4) The loop-closure result reports the error at the loop frame separately from the global ATE, because the two disagree and the disagreement is the finding. (5) The fusion table varies the prior’s accuracy and bias rather than quoting one configuration, because a single configuration cannot distinguish “fusion helps” from “the prior was good”.

The lab code

# lab_2_1_5.py -- visual odometry, loop closure, and pose-graph correction
#
# Frame convention, used everywhere below. $T_wc$ is CAMERA-TO-WORLD: it places the
# camera in the world. A world point $X_w$ appears in the camera as
#     $X_c$ = R^T ($X_w$ - p) = ($X_w$ - p) @ R,
# with +z into the scene (OpenCV's convention, not the graphics -z one).
import time
import numpy as np
import torch
import cv2

dtype = torch.float64
FX = FY = 500.0
CX, CY = 320.0, 240.0
W, H = 640, 480

def exp_so3(w):
	w = torch.as_tensor(w, dtype=dtype)
	th = w.norm()
	if float(th) < 1e-12:
		return torch.eye(3, dtype=dtype)
	a = w / th
	Kx = torch.tensor([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]], dtype=dtype)
	return torch.eye(3, dtype=dtype) + torch.sin(th)*Kx + (1-torch.cos(th))*(Kx@Kx)

def pose(axis, t):
	T = torch.eye(4, dtype=dtype)
	T[:3,:3] = exp_so3(torch.tensor(axis, dtype=dtype))
	T[:3,3] = torch.tensor(t, dtype=dtype)
	return T

# ---------------------------------------------------------------- scene
# The landmarks are TEXTURED PATCHES, not bare points. ORB needs corners; a scene of
# 3D points alone renders to a blank image with nothing to detect. Each landmark owns a
# fixed random 7x7 binary patch, so it looks the same in every frame.
$N_LM$ = 260
_rng = np.random.default_rng(0)
LM = torch.tensor(np.column_stack([
	_rng.uniform(-0.32, 0.32, $N_LM$),
	_rng.uniform(-0.24, 0.24, $N_LM$),
	_rng.uniform(0.40, 1.50, $N_LM$),
]), dtype=dtype)
PATCHES = [(np.random.default_rng(1000+i).integers(0, 2, (7,7))*255).astype(np.uint8)
           for i in $range(N_LM$)]

def $render(T_wc$, sigma_px=0.7, seed=0):
	"""Render the patches. Returns (image, pixels, landmark ids, depths).
	sigma_px jitters where each patch is drawn: that is the sensor noise the feature
	detector has to see through."""
	r = np.random.default_rng(seed)
	Xc = (LM - $T_wc$[:3,3]) @ $T_wc$[:3,:3]
	Z = Xc[:,2].numpy()
	ok = Z > 1e-6
	Zs = np.where(ok, Z, 1.0)
	u = FX*Xc[:,0].numpy()/Zs + CX
	v = FY*Xc[:,1].numpy()/Zs + CY
	img = np.full((H, W), 128, np.uint8)
	ids, px, dep = [], [], []
	for i in $range(N_LM$):
		if not ok[i]:
			continue
		uu = u[i] + r.normal(0, sigma_px)
		vv = v[i] + r.normal(0, sigma_px)
		if not (4 < uu < W-4 and 4 < vv < H-4):
			continue
		x, y = int(round(uu)), int(round(vv))
		img[y-3:y+4, x-3:x+4] = PATCHES[i]
		ids.append(i); px.append([uu, vv]); dep.append(Z[i])
	return img, np.array(px, np.float64), np.array(ids, int), np.array(dep, np.float64)

$N_FRAMES$ = 120

def $trajectory(n=N_FRAMES$, amp=0.22, yaw_amp=0.55):
	"""A yaw sweep out and back. The return leg is the loop: frame n-1 revisits the
	viewpoint of frame 0, so a loop constraint genuinely exists to be found.
	The amplitudes give a median frame-to-frame translation of 9.3 mm. That matters:
	a *relative* error percentage needs a baseline well above the noise, and at 4 mm
	steps the percentage measures the noise, not the estimator."""
	out = []
	for k in range(n):
		s = k/(n-1)
		T = torch.eye(4, dtype=dtype)
		T[:3,:3] = (exp_so3(torch.tensor([0.0, yaw_amp*np.sin(2*np.pi*s), 0.0], dtype=dtype))
		            @ exp_so3(torch.tensor([0.08*np.sin(3*np.pi*s), 0.0, 0.0], dtype=dtype)))
		T[:3,3] = torch.tensor([amp*np.sin(2*np.pi*s), 0.09*np.sin(4*np.pi*s), 0.0], dtype=dtype)
		out.append(T)
	return out

# ---------------------------------------------------------------- PnP
def $pnp_3d2d(X_ref$, s, iters=60, tol=1e-11):
	"""Rigid (R, t) with $X_cam$ = R $X_ref$ + t from 2D pixels s, Gauss-Newton on
	reprojection. Left perturbation T <- exp(xi) T.

	Two things here are easy to get wrong, and both fail silently:
	  * the residual must be INTERLEAVED (u0,v0,u1,v1,...) to match J.reshape(-1,6).
	    Stacking all-u then all-v pairs a correct Jacobian with a differently ordered
	    residual, and the solve wanders instead of converging.
	  * the (u, rotation) block for normalised coordinates is [[-xy, 1+x^2],[-1-y^2, xy]],
	    so in pixels the diagonal terms carry f^2."""
	R = torch.eye(3, dtype=dtype); t = torch.zeros(3, dtype=dtype)
	for _ in range(iters):
		Xc = $X_ref$ @ R.T + t
		Z = Xc[:,2].clamp(min=1e-9)
		X, Y = Xc[:,0], Xc[:,1]
		u = FX*X/Z + CX; v = FY*Y/Z + CY
		r = torch.stack([u - s[:,0], v - s[:,1]], -1).reshape(-1)
		J = torch.$zeros(len(X_ref$), 2, 6, dtype=dtype)
		J[:,0,0] = FX/Z;  J[:,0,2] = -FX*X/Z**2
		J[:,0,3] = -FX*X*Y/Z**2; J[:,0,4] = FX*(1 + X**2/Z**2); J[:,0,5] = -FX*Y/Z
		J[:,1,1] = FY/Z;  J[:,1,2] = -FY*Y/Z**2
		J[:,1,3] = -FY*(1 + Y**2/Z**2); J[:,1,4] = FY*X*Y/Z**2; J[:,1,5] = FY*X/Z
		Jf = J.reshape(-1, 6)
		xi = torch.linalg.solve(Jf.T@Jf + 1e-12*torch.eye(6, dtype=dtype), -(Jf.T@r))
		dR = exp_so3(xi[3:])
		R = dR @ R
		t = dR @ t + xi[:3]
		if float(xi.norm()) < tol:
			break
	return R, t

# ---------------------------------------------------------------- canaries
print("=== canaries: the machinery, before the experiment ===")

Xc_can = torch.tensor(np.column_stack([_rng.uniform(-0.25,0.25,150),
                                       _rng.uniform(-0.20,0.20,150),
                                       _rng.uniform(0.40,1.40,150)]), dtype=dtype)
$T_a$ = pose([0.18,-0.12,0.09], [0.10,-0.04,0.22])          # a FAR pose, not a nudge
Xc_b = Xc_can @ $T_a$[:3,:3].T + $T_a$[:3,3]
$s_exact$ = torch.stack([FX*Xc_b[:,0]/Xc_b[:,2] + CX, FY*Xc_b[:,1]/Xc_b[:,2] + CY], -1)
$R_b$, $t_b$ = pnp_3d2d(Xc_can, $s_exact$)
print(f"C1 PnP from the identity to a far pose, exact pixels: rot {float((R_b-T_a[:3,:3]).abs().max()):.1e}, "
      f"t {float((t_b-T_a[:3,3]).abs().max()):.1e}   (expect ~1e-15)")

# The gate above is blind to the Jacobian's *shape*: with zero residual the answer is a
# fixed point for any J, and a mildly wrong J still walks there. What a wrong J costs is
# the basin of convergence, so the canary must negate one entry and see the answer move.
def $pnp_count(X_ref$, s, iters=60, flip=None):
	R = torch.eye(3, dtype=dtype); t = torch.zeros(3, dtype=dtype)
	for it in range(iters):
		Xc = $X_ref$ @ R.T + t
		Z = Xc[:,2].clamp(min=1e-9); X, Y = Xc[:,0], Xc[:,1]
		u = FX*X/Z + CX; v = FY*Y/Z + CY
		r = torch.stack([u-s[:,0], v-s[:,1]],-1).reshape(-1)
		J = torch.$zeros(len(X_ref$),2,6,dtype=dtype)
		J[:,0,0]=FX/Z; J[:,0,2]=-FX*X/Z**2
		J[:,0,3]=-FX*X*Y/Z**2; J[:,0,4]=FX*(1+X**2/Z**2); J[:,0,5]=-FX*Y/Z
		J[:,1,1]=FY/Z; J[:,1,2]=-FY*Y/Z**2
		J[:,1,3]=-FY*(1+Y**2/Z**2); J[:,1,4]=FY*X*Y/Z**2; J[:,1,5]=FY*X/Z
		if flip: J[:,flip[0],flip[1]] = -J[:,flip[0],flip[1]]
		Jf = J.reshape(-1,6)
		xi = torch.linalg.solve(Jf.T@Jf + 1e-12*torch.eye(6,dtype=dtype), -(Jf.T@r))
		dR = exp_so3(xi[3:]); R = dR@R; t = dR@t + xi[:3]
		if float(xi.norm()) < 1e-11: return it+1, R, t
	return iters, R, t
$n_ok$, $R_ok$, _ = pnp_count(Xc_can, $s_exact$)
$n_bad$, $R_bad$, _ = pnp_count(Xc_can, $s_exact$, flip=(0,4))
print(f"C2 same pose, one Jacobian entry negated: ${n_ok}$ iterations vs ${n_bad}$, "
      f"rot err {float((R_ok-T_a[:3,:3]).abs().max()):.1e} vs {float((R_bad-T_a[:3,:3]).abs().max()):.1e}")

T0c, T1c = pose([0,0,0.10],[0.02,0,0]), pose([0.03,-0.02,0.14],[0.06,0.01,0])
Xp = (LM[:60] - T0c[:3,3]) @ T0c[:3,:3]
Xq = (LM[:60] - T1c[:3,3]) @ T1c[:3,:3]
$R_d$ = T1c[:3,:3].T @ T0c[:3,:3]
$t_d$ = T1c[:3,:3].T @ (T0c[:3,3] - T1c[:3,3])
dT_c = torch.eye(4, dtype=dtype); dT_c[:3,:3] = $R_d$; dT_c[:3,3] = $t_d$
print(f"C3 the relative transform maps PREV-camera points into CUR-camera: "
      f"residual {float(((Xp@$R_d$.T + $t_d$) - Xq).abs().max()):.1e}")
print(f"C4 chain is $T_k$ = T_(k-1) @ inv(dT): err {float((T0c@torch.linalg.inv(dT_c) - T1c).abs().max()):.1e}"
      f"   | the plausible-looking dT @ T_(k-1): err {float((dT_c@T0c - T1c).abs().max()):.1e}")

# ---------------------------------------------------------------- ORB
_orb = cv2.ORB_create(nfeatures=800, fastThreshold=10)
_bf = cv2.BFMatcher(cv2.NORM_HAMMING)

def orb_features(img):
	kp, des = _orb.detectAndCompute(img, None)
	if des is None:
		return np.zeros((0,2)), np.zeros((0,32), np.uint8)
	return np.array([k.pt for k in kp], np.float64), des

def match_features(da, db, ratio=0.75):
	"""Index pairs (ia, ib). ratio=None keeps every nearest neighbour: that is the
	control arm of H1."""
	if len(da) < 2 or len(db) < 2:
		return np.zeros((0,2), int)
	pairs = _bf.knnMatch(da, db, k=2)
	if ratio is None:
		return np.array([[m[0].queryIdx, m[0].trainIdx] for m in pairs], int)
	return np.array([[m.queryIdx, m.trainIdx] for m,n in pairs if m.distance < ratio*n.distance], int)

def depth_lookup(kp, px, ids, Z, tol=2.5):
	"""Depth at each keypoint's pixel, from the rendered depth buffer. Returns (depth, j)
	where -1 marks a keypoint with no landmark nearby -- the depth hole a real sensor
	also has. Several keypoints may land on one patch; that is what a real depth lookup
	does too, and it does not bias the solve, because each row is still the pair
	(pixel, depth-at-that-pixel)."""
	if len(kp) == 0 or len(px) == 0:
		return np.full(len(kp), -1.0), np.zeros(len(kp), int)
	d = np.linalg.norm(kp[:,None,:] - px[None,:,:], axis=2)
	j = d.argmin(1)
	out = np.where(d[np.arange(len(kp)), j] < tol, Z[j] if len(j) else 0.0, -1.0)
	return out, j

def $ransac_pnp(X_ref$, s, thresh_px=3.0, iters=60, rng=None, min_inl=6):
	"""Three-point RANSAC around PnP. Returns (R, t, inlier_mask) or None."""
	n = $len(X_ref$)
	if n < min_inl:
		return None
	best = None
	for _ in range(iters):
		idx = rng.choice(n, 3, replace=False)
		try:
			R, t = $pnp_3d2d(X_ref[idx$], s[idx], iters=15)
		except Exception:
			continue
		Xc = $X_ref$ @ R.T + t
		if bool((Xc[:,2] <= 1e-6).any()):
			continue
		u = FX*Xc[:,0]/Xc[:,2] + CX; v = FY*Xc[:,1]/Xc[:,2] + CY
		e = torch.sqrt((u - s[:,0])**2 + (v - s[:,1])**2)
		inl = e < thresh_px
		c = int(inl.sum())
		if best is None or c > best[0]:
			best = (c, inl)
	if best is None or best[0] < min_inl:
		return None
	inl = best[1]
	R, t = $pnp_3d2d(X_ref[inl$], s[inl], iters=40)
	return R, t, inl

# ---------------------------------------------------------------- VO
def vo(poses, ratio=0.75, use_ransac=True, corrupt=0.0, freeze=None, seed=0,
       want_outliers=False):
	"""Integrate relative poses along the sequence.

	The 3D side of each correspondence comes from the PREVIOUS frame's depth channel,
	back-projected through K at the keypoint's own pixel, which is what an RGB-D system
	actually has. Depth is therefore load-bearing: corrupt it and the pose estimate
	moves."""
	rng = np.random.default_rng(seed)
	T = torch.eye(4, dtype=dtype)
	est = [T.clone()]
	$p_img$, $p_px$, $p_ids$, $p_Z$ = render(poses[0], seed=seed*1000)
	$p_kp$, $p_des$ = $orb_features(p_img$)
	$p_dep$, _ = $depth_lookup(p_kp$, $p_px$, $p_ids$, $p_Z$)
	stats = []
	rej = tot = 0
	for k in range(1, len(poses)):
		img, px, ids, Z = render(poses[k], seed=seed*1000+k)
		if freeze is not None and freeze[0] <= k < freeze[1]:
			Z = np.full_like(Z, float(np.mean(Z)))
		kp, des = orb_features(img)
		pairs = $match_features(p_des$, des, ratio)
		$n_match$ = len(pairs)
		dT = None; $n_inl$ = 0
		if $n_match$ >= 6:
			ia, ib = pairs[:,0], pairs[:,1]
			ok = $p_dep[ia$] > 0
			ia, ib = ia[ok], ib[ok]
			is_bad = np.zeros(len(ia), bool)
			if corrupt > 0 and len(ia):
				nb = int(len(ia)*corrupt)
				bad = rng.choice(len(ia), nb, replace=False)
				ib = ib.copy(); ib[bad] = rng.permutation(ib[bad])
				is_bad[bad] = True
			if len(ia) >= 6:
				dp = torch.$tensor(p_dep[ia$], dtype=dtype)
				$X_ref$ = torch.stack([(torch.$tensor(p_kp[ia$,0],dtype=dtype)-CX)*dp/FX,
				                     (torch.$tensor(p_kp[ia$,1],dtype=dtype)-CY)*dp/FY, dp], -1)
				$s_cur$ = torch.tensor(kp[ib], dtype=dtype)
				if use_ransac:
					r = $ransac_pnp(X_ref$, $s_cur$, rng=rng)
					if r is not None:
						R, t, inl = r
						dT = torch.eye(4, dtype=dtype); dT[:3,:3] = R; dT[:3,3] = t
						inl_np = np.asarray(inl)
						$n_inl$ = int(inl_np.sum())
						if want_outliers and is_bad.any():
							rej += int((is_bad & ~inl_np).sum()); tot += int(is_bad.sum())
				else:
					R, t = $pnp_3d2d(X_ref$, $s_cur$)
					dT = torch.eye(4, dtype=dtype); dT[:3,:3] = R; dT[:3,3] = t
					$n_inl$ = $len(X_ref$)
		if dT is not None:
			T = T @ torch.linalg.inv(dT)          # dT maps prev-camera -> cur-camera
		est.append(T.clone())
		stats.$append((n_match$, $n_inl$))
		$p_img$, $p_px$, $p_ids$, $p_Z$ = img, px, ids, Z
		$p_kp$, $p_des$ = kp, des
		$p_dep$, _ = depth_lookup(kp, px, ids, Z)
	return est, stats, (rej, tot)

def monocular(poses, scale0=0.010, seed=0):
	"""Depth-free VO: relative rotation from the essential matrix, translation up to an
	unknown scale. scale0 fixes the metric size of the FIRST step, which is the only
	external information a pure monocular system would have; everything after inherits
	whatever error that guess carries."""
	T = torch.eye(4, dtype=dtype)
	est = [T.clone()]
	$p_img$, _, _, _ = render(poses[0], seed=seed*1000)
	$p_kp$, $p_des$ = $orb_features(p_img$)
	for k in range(1, len(poses)):
		img, _, _, _ = render(poses[k], seed=seed*1000+k)
		kp, des = orb_features(img)
		pairs = $match_features(p_des$, des, 0.75)
		if len(pairs) >= 8:
			ia, ib = pairs[:,0], pairs[:,1]
			p1 = ($p_kp[ia$] - np.array([CX,CY])) / np.array([FX,FY])
			p2 = (kp[ib] - np.array([CX,CY])) / np.array([FX,FY])
			A = np.column_stack([p2[:,0]*p1[:,0], p2[:,0]*p1[:,1], p2[:,0],
			                     p2[:,1]*p1[:,0], p2[:,1]*p1[:,1], p2[:,1],
			                     p1[:,0], p1[:,1], np.ones(len(p1))])
			_,_,Vt = np.linalg.svd(A)
			E = Vt[-1].reshape(3,3)
			U,S,Vt2 = np.linalg.svd(E)
			Wm = np.array([[0,-1,0],[1,0,0],[0,0,1.0]])
			$R_rel$ = U @ Wm @ Vt2
			if np.linalg.$det(R_rel$) < 0:
				$R_rel$ = U @ Wm.T @ Vt2
			$t_rel$ = U[:,2]/np.linalg.norm(U[:,2])
			dT = torch.eye(4, dtype=dtype)
			dT[:3,:3] = torch.$tensor(R_rel$, dtype=dtype)
			dT[:3,3] = torch.$tensor(t_rel*scale0$, dtype=dtype)
			T = T @ torch.linalg.inv(dT)
		est.append(T.clone())
		$p_kp$, $p_des$ = kp, des
	return est

def ate(est, truth):
	n = min(len(est), len(truth))
	e = np.array([float((est[k][:3,3] - truth[k][:3,3]).norm()) for k in range(n)])
	return float(np.sqrt(np.mean(e**2)))

def rel_err(est, truth):
	"""Mean relative translation error (fraction of the true step) and mean relative
	rotation error (degrees). The rotation error is the geodesic angle of
	dR_est @ dR_true^T; comparing absolute orientations instead would report the
	trajectory's overall tilt, not the per-step estimate's accuracy."""
	n = min(len(est), len(truth)); dt, dr = [], []
	for k in range(1, n):
		ta = float((truth[k][:3,3] - truth[k-1][:3,3]).norm())
		te = float((est[k][:3,3] - est[k-1][:3,3]).norm())
		if ta > 1e-9:
			dt.append(abs(te - ta)/ta)
		dE = (est[k-1][:3,:3].T @ est[k][:3,:3]) @ (truth[k-1][:3,:3].T @ truth[k][:3,:3]).T
		dr.append(float(np.degrees(np.arccos(np.clip(float((dE.trace()-1)/2), -1.0, 1.0)))))
	return (float(np.mean(dt)) if dt else float('nan')), (float(np.mean(dr)) if dr else float('nan'))

def path_len(tr):
	return float(sum(float((tr[k][:3,3]-tr[k-1][:3,3]).norm()) for k in range(1,len(tr))))

poses = trajectory()

print("\n=== part 1: does ORB see the scene at all? ===")
img0, px0, ids0, Z0 = render(poses[0], seed=0)
kp0, des0 = orb_features(img0)
d0 = np.linalg.norm(kp0[:,None,:] - px0[None,:,:], axis=2).min(1) if len(px0) else np.full(len(kp0), 1e9)
print(f"frame 0: {len(ids0)} landmarks rendered, {len(kp0)} ORB keypoints, "
      f"{int((d0<2.5).sum())} within 2.5 px of a landmark")
steps = [float((poses[k][:3,3]-poses[k-1][:3,3]).norm()) for k in range(1,len(poses))]
print(f"frame-to-frame translation: median {1000*np.median(steps):.1f} mm, max {1000*max(steps):.1f} mm")

print("\n=== part 2: VO with and without the ratio test (H1) ===")
t0 = time.time(); est_ratio, st_ratio, _ = vo(poses, ratio=0.75, seed=1); $t_ratio$ = time.time()-t0
est_plain, st_plain, _ = vo(poses, ratio=None, seed=1)
rt_r, rr_r = rel_err(est_ratio, poses)
rt_p, rr_p = rel_err(est_plain, poses)
print(f"with ratio test:    ATE {ate(est_ratio,poses):.4f} m | rel t {100*rt_r:.2f}% | rel R {rr_r:.3f} deg"
      f" | inliers {np.mean([s[1] for s in st_ratio]):.0f}/{np.mean([s[0] for s in st_ratio]):.0f}")
print(f"without ratio test: ATE {ate(est_plain,poses):.4f} m | rel t {100*rt_p:.2f}% | rel R {rr_p:.3f} deg"
      f" | inliers {np.mean([s[1] for s in st_plain]):.0f}/{np.mean([s[0] for s in st_plain]):.0f}")

print("\n=== part 3: monocular, no depth at all (H2) ===")
est_mono = monocular(poses, seed=1)
pt, pr, pm = path_len(poses), path_len(est_ratio), path_len(est_mono)
print(f"path length: true {pt:.4f} m | RGB-D {pr:.4f} m | monocular {pm:.4f} m")
print(f"scale error: RGB-D {100*abs(pr-pt)/pt:.2f}% | monocular {100*abs(pm-pt)/pt:.2f}%")
print(f"ATE: RGB-D {ate(est_ratio,poses):.4f} m | monocular {ate(est_mono,poses):.4f} m")
print("  scale error vs path length (is it drift, or is it the prior?):")
print(f"  {'frames':>7} {'true path':>10} {'RGB-D':>8} {'mono':>8}")
for nf in (20, 40, 60, 90, 120):
	a, b_, c_ = path_len(poses[:nf]), path_len(est_ratio[:nf]), path_len(est_mono[:nf])
	print(f"  {nf:7d} {a:10.4f} {100*abs(b_-a)/a:7.2f}% {100*abs(c_-a)/a:7.2f}%")
print("  monocular scale error as a function of the single scale prior:")
print(f"  {'prior step':>11} {'path':>9} {'scale err':>10}")
for s0 in (0.005, 0.0075, 0.010, 0.015, 0.020):
	em = monocular(poses, scale0=s0, seed=1)
	p = path_len(em)
	print(f"  {s0*1000:9.2f}mm {p:9.4f} {100*abs(p-pt)/pt:9.2f}%")

print("\n=== part 4: break-its ===")
est_bad, st_bad, (rej, tot) = vo(poses, ratio=0.75, corrupt=0.30, seed=1, want_outliers=True)
print(f"B1 30% corrupted matches: ATE {ate(est_bad,poses):.4f} m "
      f"(clean {ate(est_ratio,poses):.4f} m) | outliers rejected {rej}/{tot} = {100*rej/tot:.1f}%")
est_fz, _, _ = vo(poses, ratio=0.75, freeze=(40, 50), seed=1)
print(f"B3 depth frozen on frames 40-49: ATE {ate(est_fz,poses):.4f} m (clean {ate(est_ratio,poses):.4f} m)")
_, px_f, _, $Z_f$ = render(poses[40], seed=1)
print(f"B3 depth variance frame 40: live {float(np.$var(Z_f$)):.3e}, frozen "
      f"{float(np.var(np.$full_like(Z_f$, np.$mean(Z_f$)))):.3e}")

print("\n=== part 5: loop closure (H3) ===")
def loop_candidate(est, $k_min=40$):
	best = None
	for k in $range(k_min$, len(est)):
		d = float((est[k][:3,3]-est[0][:3,3]).norm())
		if best is None or d < best[1]: best = (k, d)
	return best
$k_l$, $d_l$ = loop_candidate(est_ratio)
print(f"loop candidate: frame ${k_l}$, estimated {d_l*1000:.2f} mm from start "
      f"(true {float((poses[k_l][:3,3]-poses[0][:3,3]).norm())*1000:.2f} mm)")

def pose_graph(est, $k_l$, loop_meas):
	"""Least-squares pose graph, 1-D per axis: odometry edges ($x_{i+1}$ - $x_i$ = $d_i$),
	one loop edge ($x_k$ - $x_0$ = loop_meas), and $x_0$ pinned. This is the closed form of
	the 4-DOF case with roll and pitch held fixed, which is the VINS-Mono formulation."""
	N = $k_l$ + 1
	X = np.array([[float(est[i][:3,3][a]) for i in range(N)] for a in range(3)])
	sols = []
	for a in range(3):
		A, b = [], []
		for i in range(N-1):
			r = np.zeros(N); r[i] = -1; r[i+1] = 1
			A.append(r); b.append(X[a, i+1] - X[a, i])
		r = np.zeros(N); r[0] = -1; $r[k_l$] = 1        # the loop edge: node 0 to node $k_l$
		A.append(r); b.append(loop_meas[a])
		A = np.array(A); b = np.array(b)
		A = np.vstack([A, np.eye(N)[0]]); b = np.append(b, 0.0)   # pin $x_0$ = 0
		sols.append(np.linalg.lstsq(A, b, rcond=None)[0])
	out = []
	for i in range(N):
		T = est[i].clone(); T[:3,3] = torch.tensor([sols[a][i] for a in range(3)], dtype=dtype)
		out.append(T)
	return out

loop_true = (poses[0][:3,3] - $poses[k_l$][:3,3]).numpy()
before = ate(est_ratio[:$k_l+1$], poses[:$k_l+1$])
corr = pose_graph(est_ratio, $k_l$, loop_true)
after = ate(corr, poses[:$k_l+1$])
print(f"ATE frames 0..${k_l}$: before {before*1000:.2f} mm -> after {after*1000:.2f} mm "
      f"({100*(before-after)/before:+.1f}%)")
print(f"  error AT the loop frame: before {float((est_ratio[k_l][:3,$3]-poses[k_l$][:3,3]).norm())*1000:.2f} mm"
      f" -> after {float((corr[k_l][:3,$3]-poses[k_l$][:3,3]).norm())*1000:.2f} mm")
print("  the loop edge is an ABSOLUTE constraint on one node; it cannot undo the error")
print("  the middle of the chain accumulated, so the global ATE barely moves.")
rk = np.random.default_rng(0)
for sig in (0.002, 0.005, 0.010, 0.020):
	a = [ate(pose_graph(est_ratio, $k_l$, loop_true + rk.normal(0, sig, 3)), poses[:$k_l+1$]) for _ in range(30)]
	print(f"  loop sigma {sig*1000:4.0f} mm: ATE {1000*np.mean(a):.2f} mm")

print("\n=== part 6: fusion with a kinematic prior (H4) ===")
def fuse(est, kin, $w_vo$, $w_kin$):
	out = []
	for e, g in zip(est, kin):
		T = e.clone(); T[:3,3] = ($w_vo*e$[:3,3] + $w_kin*g$[:3,3])/(w_vo+w_kin)
		out.append(T)
	return out
SIG_VO = ate(est_ratio, poses)/np.sqrt(3)   # per-axis, the number a system would report
print(f"VO ATE {ate(est_ratio,poses):.4f} m (per-axis sigma {SIG_VO:.4f} m)")
print(f"{'bias':>6} {'sig_kin':>8} {'kin ATE':>9} {'50/50':>8} {'honest IVW':>11}  beats kin?")
for bias in (0.0, 0.005, 0.010, 0.020, 0.040):
	for sig in (0.003, 0.012):
		rk2 = np.random.default_rng(7); kin = []
		for k, T in enumerate(poses):
			K_ = T.clone()
			b = torch.tensor([bias*np.sin(2*np.pi*k/len(poses)), 0.0, 0.0], dtype=dtype)
			K_[:3,3] = K_[:3,3] + b + torch.tensor(rk2.normal(0, sig, 3), dtype=dtype)
			kin.append(K_)
		ak = ate(kin, poses)
		a50 = ate(fuse(est_ratio, kin, 1.0, 1.0), poses)
		aiv = ate(fuse(est_ratio, kin, 1.0/SIG_VO**2, 1.0/sig**2), poses)
		print(f"{bias:6.3f} {sig:8.3f} {ak:9.4f} {a50:8.4f} {aiv:11.4f}  {'YES' if aiv < ak else 'no'}")
ev = np.array([float((est_ratio[k][:3,3]-poses[k][:3,3]).norm()) for k in range(len(poses))])
dv = np.array([(est_ratio[k][:3,3]-poses[k][:3,3]).numpy() for k in range(len(poses))])
print(f"VO error autocorrelation lag-1 {np.corrcoef(dv[:-1].ravel(), dv[1:].ravel())[0,1]:+.3f} "
      f"(a random walk; this is what fusion has to compete with)")

print("\n--- lab 2.1.5 summary ---")
print(f"H1 ratio test: rel t {100*rt_r:.2f}% vs {100*rt_p:.2f}% (reduction {100*(rt_p-rt_r)/rt_p:+.1f}%)")
print(f"H2 scale error: RGB-D {100*abs(pr-pt)/pt:.2f}% vs monocular {100*abs(pm-pt)/pt:.2f}%")
print(f"H3 loop closure: ATE {before*1000:.2f} -> {after*1000:.2f} mm ({100*(before-after)/before:+.1f}%)")
print(f"B1 corrupt 30%: ATE {ate(est_bad,poses):.4f} m, {100*rej/tot:.1f}% of outliers rejected")
print(f"B3 frozen depth: ATE {ate(est_fz,poses):.4f} m")
print(f"runtime: {1000*t_ratio/len(poses):.1f} ms/frame")

Cheat sheet

Exercises with solutions

E 2.1.5.1. Derive the epipolar constraint from the coplanarity of $\mathbf{X}_{C_1}$, $\mathbf{X}_{C_2}$, and $\mathbf{t}_{21}$.

S 2.1.5.1. Coplanarity means the scalar triple product is zero: $\mathbf{X}_{C_2}^\top (\mathbf{t}_{21} \times \mathbf{X}_{C_1}) = 0$. The cross product is $\widehat{\mathbf{t}}_{21} \mathbf{X}_{C_1}$, and $\mathbf{X}_{C_2} = R_{21}\mathbf{X}_{C_1} + \mathbf{t}_{21}$, but the $\mathbf{t}_{21}$ term vanishes in the triple product because $\mathbf{t}_{21}^\top \widehat{\mathbf{t}}_{21} \mathbf{X}_{C_1} = 0$. Substituting normalized coordinates gives $\tilde{\mathbf{p}}_2^\top \widehat{\mathbf{t}}_{21} R_{21} \tilde{\mathbf{p}}_1 = 0$. The lesson: the epipolar constraint is a statement about three coplanar vectors, not a special property of cameras.

E 2.1.5.2. A frame pair yields 800 putative matches. The ratio test at $\rho=0.75$ leaves 240. RANSAC with the five-point solver and $p=0.99$ is run. If $w=0.45$, how many iterations are required, and what happens if the implementation caps at 200?

S 2.1.5.2. $0.45^5 = 0.01845$. $N = \log(0.01)/\log(1-0.01845) \approx 4.605/0.01862 \approx 247$. A 200-iteration cap gives confidence $1-(1-0.01845)^{200} \approx 1-0.024 = 0.976$, so the pipeline fails to find a consensus sample about 2.4% of the time. The lesson: hard-coded iteration caps are silent failure points; compute the budget from the measured inlier ratio.

E 2.1.5.3 (transfer). You have a stereo camera with baseline $b=0.12$ m and focal length $f=500$ px. A point has disparity $d=40$ px. Compute its depth. Does stereo VO suffer monocular scale ambiguity?

S 2.1.5.3. $Z = f b / d = 500 \cdot 0.12 / 40 = 1.5$ m. No: the known baseline provides metric scale, so stereo VO does not suffer the monocular scale ambiguity. The lesson: scale ambiguity is a property of the sensor configuration, not of visual odometry as such.

E 2.1.5.4 (diagnostic). A VO pipeline reports a rotation matrix that is not orthonormal. Name two mechanisms that could produce this and the fix for each.

S 2.1.5.4. (1) Averaging or interpolating rotations component-wise, which leaves the manifold; fix by slerp or local-tangent optimization. (2) Accumulating small rotation updates without re-orthonormalization, which lets numerical drift accumulate; fix by projecting onto $SO(3)$ via SVD after each update. The lesson: rotations are not vectors; every operation that adds or averages them must be manifold-aware.

E 2.1.5.5 (transfer). The running case’s arm moves the camera purely in translation along the optical axis toward a fronto-parallel table. Will feature-based VO estimate translation well? Explain using parallax.

S 2.1.5.5. No. The baseline is parallel to the optical axis, so the triangulation angle is near zero and depth is poorly conditioned; the epipolar geometry is near-degenerate. VO will estimate rotation reliably but translation/depth poorly. The fix is to add lateral motion or use depth from the RGB-D sensor. The lesson: VO accuracy depends on motion geometry, not only on feature count.

E 2.1.5.6 (diagnostic). A loop closure is accepted on appearance alone. The pose graph optimization produces a map where the cube and the bin overlap. Diagnose the failure and state the missing stage.

S 2.1.5.6. Perceptual aliasing: two visually similar tabletop patches were merged. The missing stage is geometric verification (RANSAC inlier count on feature matches) plus consistency checking of the loop constraint against the odometry chain. The lesson: appearance proposes loop candidates; geometry and consistency decide.

E 2.1.5.7. Write the inverse-variance fusion of two scalar position estimates $a=0.048$ m with $\sigma_a=0.004$ m and $b=0.050$ m with $\sigma_b=0.001$ m. Which source dominates, and why?

S 2.1.5.7. $x = (\sigma_b^2 a + \sigma_a^2 b)/(\sigma_a^2+\sigma_b^2) = (10^{-6}\cdot 0.048 + 1.6\times10^{-5}\cdot 0.050)/(1.7\times10^{-5}) \approx (4.8\times10^{-8} + 8.0\times10^{-7})/1.7\times10^{-5} \approx 0.0499$ m. The kinematic estimate dominates because its variance is 16 times smaller. The lesson: fusion weights are inverse variances, not arbitrary blends.

E 2.1.5.8 (transfer). The eye-in-hand camera’s depth channel freezes for 20 frames. Design the fallback: which VO mode runs, what happens to scale, and what should the pipeline report.

S 2.1.5.8. The pipeline falls back to monocular VO (or, if an IMU is present, VIO). Scale becomes unobservable from vision alone, so the estimate relies on the kinematic prior or the last valid scale; the pose covariance must be inflated. The pipeline reports a depth_invalid flag and the covariance inflation, not a silent continuation. The lesson: graceful degradation is a reporting problem as much as an estimation problem.

Exit test

You are ready for Lesson 2.3.3 when you can, without notes:

1. Write the epipolar constraint and identify $E$, $\tilde{\mathbf{p}}_1$, and $\tilde{\mathbf{p}}_2$.
2. State the five degrees of freedom encoded by $E$ and derive the scale ambiguity.
3. Explain why the eight-point algorithm fails on coplanar points and which solver replaces it.
4. Describe ORB’s two components and why binary descriptors are cheap to match.
5. Compute the RANSAC iteration count from an inlier ratio and a sample size.
6. Distinguish feature-based, direct, and semi-direct VO by their error functions.
7. State the photometric-consistency assumption and two violations.
8. Explain the roles of keyframes and local bundle adjustment.
9. Describe bag-of-words loop detection and the necessity of geometric verification.
10. Set up a scalar pose-graph optimization with a loop-closure edge.
11. Explain why monocular VO is scale-ambiguous and name three resolutions.
12. Write the kinematic prior for the eye-in-hand camera and explain how it fuses with VO.
13. State the inverse-variance fusion rule and explain why rotations must be fused on the manifold.
14. Design a drift evaluation against forward-kinematics ground truth and name the metrics.
15. Diagnose a loop-closure failure caused by perceptual aliasing.

Cross-references. 2.1.1 supplies the projection model restated in §L.A.1. 1.1.1 supplies the forward-kinematics prior restated in §L.E.2. 2.1.4 supplies the geometric-registration mindset contrasted with photometric error in §L.C.2. 2.3.3 consumes the relative-pose estimator to maintain object-pose estimates during contact, when the kinematic chain alone is insufficient. Phase 3 consumes the map and pose estimates as spatial grounding for VLA policies.

Ready. The next lesson to write is 2.3.3 — Contact-Rich Manipulation. Say “next” to generate it.