Visual Navigation Workbook

The projection u = f_x(X/Z) + c_x maps a camera-frame X coordinate to a pixel column. X/Z = 0.075 is the tangent angle; multiplying by f_x=800 converts it from radians to pixels; adding c_x=320 shifts from image center to image corner. The v-coordinate would be 800×(0.1/4.0)+240 = 260 px.

Physical focal length × sensor density = focal length in pixels. A 50mm lens on a 35mm full-frame sensor (36mm wide, 8000px) would give f_x = (8000/0.036)×0.05 ≈ 11,111 px. Focal length in pixels captures the combined effect of lens choice and sensor resolution.

The back-projected ray direction in camera coordinates is [x_n, y_n, 1]^T = [(u−c_x)/f_x, (v−c_y)/f_y, 1]^T = [0.2, 0, 1]^T. At depth Z=5m, the 3D point is [1.0, 0, 5]^T. The ray direction is the unit vector K⁻¹[u,v,1]^T normalized.

Radial distortion moves points outward (k₁>0, barrel distortion) or inward (k₁<0, pincushion). Here k₁=0.1 gives a 2.5% outward push at r=0.5. Real wide-angle cameras can have k₁ ≈ −0.3 (strong pincushion). Calibration tools (OpenCV, Kalibr) estimate k₁, k₂, k₃ simultaneously with f_x, f_y, c_x, c_y.

Both points project to the same pixel because X/Z = 0.075 is the same for both. The ray passes through A=(0.15, ?, 2) and B=(0.45, ?, 6) — three times farther gives three times larger X. This is the fundamental depth ambiguity: you know the ratio X/Z but not X or Z individually from a single frame.

M = ∑[[I_x², I_xI_y],[I_xI_y, I_y²]]. With I_x=5 and I_y=0 at every pixel in a 3×3 window (9 pixels): M = 9×[[25,0],[0,0]] = [[225,0],[0,0]]. The eigenvalues are 225 and 0: large in one direction (strong gradient perpendicular to the edge), zero in the other (no variation along the edge). This is a vertical edge (λ₁≫λ₂=0). Option C is qualitatively correct: one large eigenvalue, one zero.

R > 0 indicates a corner. R < 0 indicates an edge (det<k·tr²). R ≈ 0 indicates a flat region. For the threshold, practitioners keep keypoints with R > α · max(R) across the image, where α ≈ 0.01.

Lowe's test: accept if d₁/d₂ < 0.8. Here 0.689 < 0.8, so the match is accepted. The intuition: if the best match is much closer than the second-best, it's probably correct. If the ratio is close to 1 (two equally plausible matches), the correspondence is ambiguous (e.g., repeated textures) and should be rejected. Lowe showed that 0.8 keeps 95% of correct matches and rejects 90% of incorrect ones.

det(M) = λ₁λ₂ and tr(M) = λ₁+λ₂. With det=32>0 and tr=13, both eigenvalues are positive — this is a corner region. The Harris response is R = 32 − 0.04×13² = 32 − 6.76 = 25.24 > 0, confirming a corner.

FAST-9 requires 9 consecutive pixels in the 16-pixel Bresenham circle all brighter or all darker than the center by a threshold. A 9-pixel arc is a little over half the circle, making it geometrically impossible for an edge (which would have brighter pixels on one side but at most 8 consecutive bright pixels). FAST-12 (12 pixels) is more conservative; ORB uses FAST-9 as its detector.

The epipolar constraint is satisfied: this match is geometrically consistent with the given camera motion. A horizontal baseline (t along x) produces horizontal epipolar lines — y₂ must have the same v as y₁ (both have normalized v=0 here).

With a pure horizontal baseline and no rotation, the depth Z = baseline / disparity (in normalized coords). In pixel coords: Z = f · b / (u₁ − u₂). This is the stereo depth formula. Doubling the baseline halves depth error; doubling the depth quadruples it.

At Z=6m (double): σ_Z = 36/60 = 0.6m — quadrupled, as expected from the Z² scaling. At Z=10m with the same setup: σ_Z = 100/60 ≈ 1.67m, which is why stereo SLAM loses depth accuracy fast at range. LiDAR has roughly linear depth error, giving it a huge advantage beyond ~10m.

Relative pose has 6 DOF (3 rotation + 3 translation). But only the direction of translation is observable from image correspondences (multiply all 3D points by λ and t by λ → same images). So translation has 2 DOF (unit sphere). Total: 3 + 2 = 5 DOF. Each correspondence provides one scalar constraint (y₂^TEy₁=0). Five constraints = 5 equations for 5 unknowns. The 8-point algorithm uses 8 correspondences and fits a general 3×3 rank-2 matrix (8 DOF before enforcing rank-2), which is over-parameterized but simpler to implement.

Disparity is proportional to baseline: doubling b doubles d. Larger disparity means the depth estimate is more precise (same sub-pixel error over a larger signal). The ZED stereo camera (12cm baseline) and the Intel D435 (5cm baseline) were designed with this tradeoff in mind.

With 70% inliers and s=2, you only need 7 iterations to have 99% confidence of hitting an all-inlier pair. The simplicity of the line fit (small s) compensates for uncertainty. Compare to the homography (s=4): w=0.7, N = ln(0.01)/ln(1−0.7⁴) = −4.605/ln(0.7599) ≈ −4.605/−0.2744 ≈ 17 iterations.

Compare to the 5-point algorithm at w=0.5: w⁵=0.03125, 1−w^s=0.96875, N = ln(0.01)/ln(0.96875) = −4.605/−0.03175 ≈ 145 iterations. The 5-point solver reduces iterations by 8× at 50% inliers. With 60% outliers (w=0.4): 8-point needs N≈17,000 iterations — effectively unusable. 5-point needs ≈600.

This inlier fraction can be used to dynamically update N during the RANSAC loop. Adaptive RANSAC starts with N=∞ and, after each iteration, updates N = log(1−p)/log(1−w^s) using the current best w. If w improves, N shrinks. PROSAC (Progressive Sample Consensus) additionally sorts correspondences by quality to try high-quality pairs first.

A 3×3 homography matrix H has 9 entries but is defined up to scale (multiply H by any λ ≠ 0 and it represents the same mapping). So it has 8 DOF. Each correspondence (x',y') ↔ (x,y) contributes 2 scalar equations (one for x', one for y' after the homogeneous divide). Four correspondences give 8 equations for 8 unknowns: exactly determined. With 3 correspondences you have 6 equations for 8 unknowns: a 2-parameter family of solutions.

Going from 95% to 99% confidence requires 34 vs. 22 iterations — only 55% more work for a significantly stronger guarantee. In practice, 500–2000 iterations are used for robustness; the formula gives the theoretical minimum. Note that increasing w from 0.6 to 0.8 would reduce N₉₉ from 34 to just 7 iterations.

The residuals are r = b − Aâ = [2−1.4, 4−4.2]^T = [0.6, −0.2]^T. Cost = 0.36 + 0.04 = 0.40. Verify: d(cost)/da = 2A^T(Aa−b) = 2(10×1.4−14) = 0 ✓. The data doesn't pass through the origin perfectly (truth would be a=1 for y=x), so the least-squares fit balances both residuals.

The true solution is x* = √3 ≈ 1.732. After one step we're at 1.75 — already very close. A second GN step: r(1.75) = 1.75²−3 = 0.0625; J=3.5; δ=−0.0179; x=1.732 — converged. GN converges quadratically near the solution for well-conditioned problems.

Damping λ=4 (equal to J^TJ) halves the step from 0.5 to 0.25. As λ→∞, δ→0 (infinitesimal gradient-descent steps). As λ→0, δ→0.5 (pure Gauss-Newton). Typical LM implementations start with λ=10⁻⁴·max(diag(J^TJ)) and multiply by 10 on failure, divide by 10 on success.

Cramer's rule: for [[a,b],[c,d]]δ=v, δ₁=(dv₁−bv₂)/det, δ₂=(av₂−cv₁)/det. With v=[[−2],[1]] and det=9: δ₂=(5×1−(−1)×(−2))/9=3/9=1/3. In practice, Cholesky factorization solves J^TJδ=−J^Tr in O(n³/3) for dense systems or sparse Cholesky for large SLAM graphs.

Information = 1/σ². Ratio = (1/0.0001) / (1/1) = 1/0.0001 = 10,000. A sensor with 100× better precision gets 10,000× more weight in the cost function. This means a single good measurement can dominate thousands of poor ones. In SLAM this matters: a well-calibrated camera (sub-pixel, σ≈1px) should heavily outweigh a poor depth prior.

(I+δ)^T(I+δ) = I + δ + δ^T + δ^Tδ. Since δ is skew-symmetric: δ + δ^T = 0. So (I+δ)^T(I+δ) = I + δ^Tδ. With ε=0.1: δ^Tδ has entries of order ε²=0.01. So the result is I + O(ε²) ≠ I. For large enough ε this is a significant violation. The exponential map avoids this: exp(δ)^Texp(δ) = I exactly.

Boxminus gives the "angular distance" between two rotations: the rotation that takes R₂ to R₁. This is the correct Riemannian geodesic distance on SO(3). The Euclidean distance ||R₁−R₂||_F is NOT a good rotation distance — it depends on the embedding in ℝ⁹ rather than the manifold geometry.

Only R·exp(δ̂) is guaranteed to be in SO(3) for any δ. exp(δ̂) ∈ SO(3) because δ̂ is skew-symmetric (the matrix exponential of a skew-symmetric matrix is orthogonal). The product of two SO(3) elements is in SO(3) (group closure). Options A and C break orthogonality for finite δ. Option D (Gram-Schmidt re-orthogonalization) works but is not the natural manifold retraction.

Each camera pose update δ ∈ ℝ⁶: three components for the rotation update (φ, applied via exp(φ̂)) and three for the translation update (ρ). In a factor graph with N camera poses and M 3D points, the full update vector δ has 6N + 3M components. A typical SLAM graph with 100 keyframes and 1000 landmarks has a 1500-dimensional update vector solved at each LM iteration.

R[0][0] = −1. This is R_z(180°): x→−x, y→−y, z→z. Sanity check: tr(R) = −1+−1+1 = −1, and arccos((−1−1)/2)=arccos(−1)=180° ✓. At 180° the formula is well-defined (sin=0 term vanishes cleanly).

200 steps × 0.3m = 60m total path. Drift = 0.5% × 60m = 0.30m. This is 0.5% of path length, consistent with the per-step error. State-of-the-art VO systems like VINS-Mono achieve ~0.5% drift over hundreds of meters. Loop closure can reduce accumulated drift to zero at the cost of a global optimization.

Monocular SfM recovers structure and motion up to a global scale. If the camera moves 1m or 1km, the image observations are identical after scaling all 3D points and the translation by the same factor. Scale can be set by: (a) a stereo frame (known baseline), (b) IMU integration (metric acceleration), (c) a known-size object, or (d) GPS. Rotation is scale-independent and fully recoverable from pure image correspondences.

0.5m of drift in 10 seconds is typical for consumer-grade IMU dead-reckoning. At t=30s: drift = ½×0.01×900 = 4.5m. This is why VIO fuses camera and IMU: the camera corrects IMU drift every few milliseconds, keeping overall drift manageable. High-end IMUs (tactical grade) have b≈0.0001 m/s², giving only 0.005m drift in 10s.

Two motivations: (1) computational efficiency — the bundle adjustment graph grows linearly with keyframe count; selecting keyframes with sufficient baseline keeps the graph manageable. (2) geometric quality — two near-identical views produce near-zero baseline, making triangulation numerically ill-conditioned (depth uncertainty diverges). The 0.5m threshold ensures a meaningful baseline for each new keyframe. ORB-SLAM3 uses a similar strategy, marginalizing redundant keyframes to bound graph size.

VIO reduces drift by 5× over VO alone. At 500m the difference is 6m — enough to miss a doorway or hit a wall. State-of-the-art VIO systems (VINS-Fusion, Kimera) achieve ~0.1–0.5% drift over hundreds of meters in indoor environments. Outdoor long-range navigation still requires GNSS or loop closures.

A word appearing in 1% of images has IDF ≈ 4.6. A word appearing in 50% of images: idf = ln(2) ≈ 0.69. A word appearing in every image: idf = ln(1) = 0 (completely suppressed). The IDF is the primary discriminability signal: rare visual words distinguish places far better than common ones (which appear everywhere from generic textures like sky or asphalt).

This visual word contributes weight 0.230 to the image's TF-IDF vector. A common word appearing 50 times but with idf=0.69 would only contribute 0.050×0.69=0.069 — 3.3× less influential despite being twice as frequent. TF-IDF correctly prioritizes distinctive rare words over common ones.

Cosine similarity 0.96 is very high — the two images share most of their visual content. A threshold of 0.7–0.8 is typically used to identify loop-closure candidates. Identical images have similarity 1.0; completely different scenes have similarity ≈ 0 (orthogonal BoW histograms in high-dimensional vocabulary space).

Candidates 1 and 2 pass geometric verification (42 and 38 > 20 threshold). Candidates 3-5 are rejected as false positives from BoW (8, 5, 3 inliers — these are coincidental visual similarity, not actual loop closures). In practice, systems often require temporal consistency too: the loop-closure candidate must be consistent across multiple consecutive frames, reducing false positives further.

DBoW2 (ORB-SLAM2) uses exactly these parameters: 10-ary tree with 6 levels = 10⁶ words. Quantizing a descriptor takes only 6 comparisons (one per level), not 10⁶. The tree structure gives O(b·d) = O(60) assignment time vs. O(10⁶) for flat k-means. This is why 1M-word vocabularies are practical in real-time SLAM.

This is the simplest pose-graph correction: uniform distribution of loop-closure error across the path. A full back-end optimizer (g2o, GTSAM) distributes the correction proportionally to odometry uncertainty at each segment — high-uncertainty segments absorb more of the correction. But even the naive equal-distribution approach dramatically reduces drift vs. no loop closure.

The squared cost is ~5× larger for u=10. At u=100: ρ_sq=5000 vs. ρ_Huber=99.5, a ratio of ~50. The Huber loss only grows linearly for large residuals, preventing a single outlier from dominating the graph. From the VNAV lesson's table: at u=10, Squared=50, Huber=9.5, Cauchy≈2.3, TLS=0.5 (fully saturated).

TLS completely suppresses outliers: once the residual exceeds the threshold c, the IRLS weight is exactly zero. This is the harshest robust cost — outliers are entirely ignored. The downside: TLS has flat regions with zero gradient, making it non-convex and prone to local minima. GNC (Graduated Non-Convexity) solves this by starting with a convex surrogate and annealing toward TLS.

The inlier has 8× more influence than the outlier after one IRLS step. In subsequent iterations, if the outlier residual grows further, its weight decreases further. IRLS converges when the weights stop changing — typically 5–15 iterations for well-conditioned problems. Cauchy weights decrease even faster: w(u)=1/(1+u²/c²); at u=8, c=1: w=1/65≈0.015 — 67× smaller than the inlier.

With squared cost: outlier fraction = 50/(50+50)=50% — the single outlier equals ALL odometry combined. With Huber: 16%. With Cauchy (c=1, u=10): ρ(10)=ln(1+100)/2≈2.3; fraction=2.3/52.3≈4.4%. With TLS (c=1): ρ(10)=0.5; fraction=0.5/50.5≈1.0%. The robust cost hierarchy correctly assigns less and less budget to the outlier.