X2.1.4

Lesson 2.1.4 — Point Clouds and Registration: The ICP, the Global Alignment, and the Geometry Behind the Pose

Week 10 · Prerequisites: 2.1.1, 2.1.2, 2.1.3, 0.2.1, 0.2.2.

Prior results used. This lesson uses 2.1.1’s pinhole model, the back-projection’s ray, and the depth’s ambiguity; 2.1.2’s 6D pose estimation and the symmetric’s ambiguity; 2.1.3’s visual servoing; 0.2.1’s SE(3) composition; and 0.2.2’s SVD. Each is restated at the point of use.

Hands forward. This lesson hands forward one capability: the learner can convert a depth image to a point cloud, estimate surface normals, register a partial observation to a full model with the ICP (point-to-point and point-to-plane), align two unknown clouds globally with FPFH plus RANSAC, understand PointNet’s permutation invariance, and diagnose the four registration bugs (wrong correspondence, local minimum, scale mismatch, wrong normals) from their residual’s symptoms — the capability that 2.1.2’s pose refinement, 2.4.1’s SLAM front-end, and 2.3.2’s grasp alignment all assume.

Running case. The same 6-DOF MuJoCo arm from 1.1.1–2.1.3, with the eye-in-hand camera’s RGB-D image of the cube at p^W = [0.5, 0.2, 0.3]. The depth channel returns Z(u, v) per pixel. The observed cloud is the back-projection of the visible face (about 10,000 points). The model is the cube’s full surface sampled at 1 mm (about 15,000 points). The registration is the recovery of T_Cam^Obj from the observed cloud to the model — the geometric analogue of 2.1.2’s PnP, with the ICP’s local refinement replacing the depth’s ICP and the global registration replacing the PnP when the correspondences are unknown.

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

§L.0.1 — Lesson map and word budget

§L.0.2 — Objectives

By the end of this lesson you will be able to do eight things:

1. State what a point cloud is, its sources (RGB-D, LiDAR, structured light, multi-view stereo), its representations (raw, voxel grid, octree, KD-tree), and its properties (permutation invariance, non-uniform density, no connectivity).
2. Convert a depth image to a point cloud by back-projection, and state the sparsity and noise that the conversion introduces.
3. Implement the ICP with nearest-neighbor correspondence and SVD-based Kabsch alignment, and state its local convergence and initial-pose dependence.
4. Implement the point-to-plane ICP variant and state why it converges faster than point-to-point in flat regions.
5. State the global registration problem and implement the FPFH plus RANSAC alignment, and state the FGR alternative.
6. State the PointNet architecture and explain why max-pooling makes the network order-invariant.
7. Diagnose the four registration bugs — wrong correspondence, local minimum, scale mismatch, wrong normals — from their residual symptoms.
8. Produce a working partial-to-full registration pipeline for the running case’s cube, with the ICP (initialized from 2.1.2), the point-to-plane variant, the global FPFH plus RANSAC, and verification (residual < 2 mm, ADD < 1 cm).

§L.0.3 — Misconception inventory

Each misconception is displaced in the section named, and each is tagged in the concept check that follows.

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

This is the twenty-fifth lesson. The warm-up draws from 2.1.2 (two steps back), 2.1.1 (three), 1.2.5 (four), and 1.2.4 (five).

Warm-up 1 (from 2.1.2, the PnP). State the PnP’s problem and the EPnP’s role. Answer: given n 2D-3D correspondences (X_i^Obj, x_i) and K, find T_Cam^Obj. The EPnP gives the O(n) solution with four control points. How this is used: §L.2’s ICP operates on 3D-3D correspondences (not 2D-3D), and its Kabsch alignment is a closed-form rigid fit (not iterative PnP). The two are complementary: PnP for the sparse correspondences from 2.1.2, ICP for the dense point clouds.

Warm-up 2 (from 2.1.1, back-projection). State the back-projection x_n = K⁻¹ x and the ray C + λ R x_n. Answer: x_n is normalized image coordinates; λ is the depth along the ray. How this is used: §L.1’s back-projection uses per-pixel Z = depth(u, v) as the ray parameter, giving the point cloud directly. The depth’s ambiguity from 2.1.1 is resolved by the measured Z.

Warm-up 3 (from 1.2.5, SAC’s replay buffer). State the buffer’s role and off-policy sample efficiency. Answer: the buffer stores past transitions and off-policy reuse gives many gradient steps per transition. How this is used: §L.3’s RANSAC uses many samples of minimal correspondences (three points each), each closed-form alignment is one “sample,” and the probability of a good sample follows the same 1 − (1 − w^m)^K formula.

Warm-up 4 (from 1.2.4, PPO’s clipping). State clipping’s role and the trust region. Answer: min(r A, clip(r) A) bounds the ratio to [1−ε, 1+ε], approximating the trust region. How this is used: §L.2’s point-to-plane linearizes the residual and iterates small steps. Each step is the trust-region analogue (bounded linearization); the clipping is the analogue of step-size control.

Part A — Point Clouds: Sources, Representations, and Back-Projection

§L.1 — Objects: the point cloud, its sources, its representations

A point cloud is a set P = {p_1, …, p_N} with p_i ∈ ℝ³. It has no connectivity, no faces, and no order. The set is permutation-invariant: any operation on the cloud must give the same result under any reordering of the points.

Properties:

• Permutation-invariance: the labels are arbitrary.
• Non-uniform density: closer surfaces are denser (perspective at 2.1.1).
• No connectivity: surface structure must be inferred from the neighborhood (normals, curvature, features).
• Sparsity: points sample the surface, not the volume; gaps are the sampling.

Sources:

• RGB-D cameras (Kinect, RealSense): structured light or time-of-flight per pixel, giving dense clouds (~300k points per 640×480 frame).
• LiDAR: laser time-of-flight, giving sparse clouds (~10k–100k points per scan), wide FOV, long range.
• Structured light: projector pattern plus camera deformation gives depth.
• Multi-view stereo: multiple images plus triangulation (the 2.1.1 disparity).

Representations:

• Raw: N × 3 (or N × 6 with normals and colors).
• Voxel grid: regular 3D grid, one point per voxel (downsampling, uniform density).
• Octree: hierarchical 8-ary partition (range queries, compression).
• KD-tree: binary space partition (nearest-neighbor queries in O(log N)).

Back-projection from a depth image:

X = (u - c_x) * Z(u, v) / f_x
Y = (v - c_y) * Z(u, v) / f_y
Z = Z(u, v)

where (u, v) is the pixel, Z(u, v) is the depth, and K is the intrinsics. The conversion is per-pixel and O(N_pixels).

Depth noise. RGB-D cameras are noisy: about ±few mm at 1 m, ±1 cm at 3 m. The noise accumulates in registration. Filtering (statistical outlier removal, radius outlier removal, bilateral filtering) is required pre-processing.

§L.1.1 — Result: the point cloud is an unstructured set; registration infers surface structure from the neighborhood

Claim. The point cloud’s permutation invariance and lack of connectivity are fundamental. Registration is the geometric alignment problem: find T ∈ SE(3) such that T P is close to Q. ICP is local (initialized at T_0); global registration uses features (FPFH) and RANSAC.

Proof sketch. The cloud is a set, and order is arbitrary. Registration finds the map T such that T P is close to Q. ICP alternates correspondence (argmin over candidate matches) and alignment (argmin over the transform); the alternation decreases the error monotonically, converging to a local minimum. Global registration samples minimal correspondences (three points), solves the closed-form alignment, and counts inliers.

Corollary. Permutation invariance is why the PointNet’s max-pooling is the right symmetry: the max over features is order-invariant, giving the deep model the point cloud’s structure.

Corollary. Density non-uniformity is why voxel downsampling is required for stable registration. Octrees and KD-trees make nearest-neighbor queries efficient.

§L.1.2 — Worked-example ladder

WE-L.1.a — Fully worked: back-projection of a pixel

Input. K = [[500, 0, 320], [0, 500, 240], [0, 0, 1]], pixel (400, 300), depth Z = 1.0 m.

Trace.

1. X = (400 − 320) × 1.0 / 500 = 80 / 500 = 0.16 m.
2. Y = (300 − 240) × 1.0 / 500 = 60 / 500 = 0.12 m.
3. Z = 1.0 m.
4. Point (0.16, 0.12, 1.0).

Output and interpretation. Pixel (400, 300) is right of and below the center; the point is at (0.16, 0.12, 1.0). The back-projection is exact when the depth is exact.

Self-explanation 1. If the depth is 2.0, what is X?

Answer. X = 80 × 2.0 / 500 = 0.32 m. The same pixel, twice the depth, gives twice the X. The pixel is a direction; the depth scales along that direction.

Self-explanation 2. If the pixel is (320, 240) (center), what is the point?

Answer. X = 0, Y = 0, Z = 1.0. The optical axis is the center; the point is on the camera’s Z axis.

WE-L.1.b — Completion problem

Input. K as above, pixel (100, 400), depth Z = 0.5 m.

Trace (final 25% as prompts).

1. X = (100 − 320) × 0.5 / 500 = −0.22 m. ⟨verify⟩
2. Y = (400 − 240) × 0.5 / 500 = 0.16 m. ⟨verify⟩
3. Z = 0.5 m. ⟨verify⟩
4. Point (−0.22, 0.16, 0.5). ⟨verify⟩

Answers. All correct.

The lesson. −0.22 is left of center; 0.16 is below center (positive Y in the standard convention).

WE-L.1.c — Faded: cloud from a 10×10 pixel patch

Setup. 10×10 patch, K as above, flat surface at Z = 1.0.

Question. Shape and size of the cloud?

You must produce the intermediate steps. The setup and the answer are given.

Answer. 100 points. The X range is (u_min − c_x) × 1/500 to (u_max − c_x) × 1/500. With u ∈ [100, 109] and c_x = 320, X ∈ [−0.44, −0.42], about 4 cm wide. The Y range is similar. The cloud is a plane at Z = 1.0 with 2 mm point spacing (1 m / 500).

The lesson. A flat patch gives a planar cloud — the easiest case for point-to-plane. Curved surfaces are harder.

WE-L.1.d — Contrastive near-miss: depth units

Correct solution. Depth in meters; K in pixels with f = 500, so points in meters.

Near-miss. Depth in millimeters (Z = 1000 for 1 m); K unchanged.

Why the near-miss produces a plausible but incorrect number. Back-projection with Z = 1000 gives points 1000× larger than expected. Registration against a meters-based model matches nearest neighbors to physically distant points, and ICP diverges or converges to a wrong pose. Symptom: residual off by the scale factor.

The discrepancy. Units in millimeters versus meters, factor 1000. The fix is unit consistency (all meters or all millimeters), not mixed.

Self-explanation 3. If both model and observed are in millimeters, is the ICP correct?

Answer. Yes — the scale is consistent, so nearest-neighbor and residuals are in the same units. Consistency is the key, not the absolute units.

Part B — ICP: Correspondence, Kabsch, Point-to-Plane, Convergence

§L.2 — Objects: the ICP’s steps, the Kabsch, the variants

The ICP (Besl and McKay 1992, tier A, S) alternates:

1. Initialize T_0.
2. Iterate from T_k:
   • Correspondence: for each p_i in P, find nearest q_j in Q (Euclidean distance).
   • Alignment: solve T minimizing Σ ‖T p_i − q_{c(i)}‖² via Kabsch.
   • Update T_{k+1} = T.
3. Stop when ΔT < ε or max_iter reached.

Kabsch closed-form via SVD:

1. Centroids p̄ = (1/N) Σ p_i, q̄ = (1/N) Σ q_i.
2. Centered p'_i = p_i − p̄, q'_i = q_i − q̄.
3. Covariance H = Σ p'_i q'_iᵀ (3×3).
4. SVD H = U S Vᵀ.
5. Rotation R = V diag(1, 1, det(V Uᵀ)) Uᵀ (the det +1 excludes reflections).
6. Translation t = q̄ − R p̄.

Point-to-plane variant:

min_T Σ [ (T p_i − q_{c(i)}) · n_{c(i)} ]²

where n_{c(i)} is the normal at q_{c(i)}. For small T, linearize T ≈ (I + [ω]×, t); the residual becomes linear in (ω, t) (six unknowns), and the least-squares is a 6×6 solve — no SVD needed. Point-to-plane converges 5–10× faster in flat regions (Rusinkiewicz and Levoy 2001, tier B, S).

Variants:

Local minima risk. The ICP basin is typically < 30° rotation, < 10% of the object’s size in translation. Global registration is required when the initial is far.

§L.2.1 — Result: ICP is local, Kabsch is closed-form, and point-to-plane is 5–10× faster

Claim. ICP’s convergence is local (basin depends on T_0). Kabsch is the closed-form optimum for the current correspondence. Point-to-plane is 5–10× faster than point-to-point in flat regions.

Proof sketch. ICP alternates two argmins; the error decreases monotonically, converging to a local minimum. Kabsch’s SVD is the exact closed-form. Point-to-plane penalizes only the normal-direction error, so sliding along the surface is free; point-to-point penalizes sliding, so it drifts toward alignment rather than convergence in flat regions.

Corollary. Point-to-point drifts along surfaces; point-to-plane does not.

Corollary. The ICP is fast enough for real-time at 10 Hz for N ~ 10⁴ points with point-to-plane and a KD-tree.

§L.2.2 — Worked-example ladder

WE-L.2.a — Fully worked: Kabsch for a two-point correspondence

Input. P = {(0, 0, 0), (1, 0, 0)}, Q = {(0, 0, 1), (1, 0, 1)} (translation by 1 in Z).

Trace.

1. Centroids p̄ = (0.5, 0, 0), q̄ = (0.5, 0, 1).
2. Centered p' = {(−0.5, 0, 0), (0.5, 0, 0)}, q' = {(−0.5, 0, 0), (0.5, 0, 0)}.
3. H = Σ p' q'ᵀ = [[0.5, 0, 0], [0, 0, 0], [0, 0, 0]].
4. SVD U = V = I, S = diag(0.5, 0, 0).
5. R = V diag(1, 1, det(I)) Uᵀ = I.
6. t = q̄ − R p̄ = (0, 0, 1).

Output and interpretation. Kabsch recovers the pure translation (0, 0, 1). For a pure translation, R = I and t = q̄ − p̄.

Self-explanation 4. Why is the Kabsch’s rotation the det-corrected version?

Answer. SVD can give a reflection (det = −1) when the correspondence is noisy. The diag(1, 1, det(V Uᵀ)) forces det = +1, excluding reflections so R ∈ SO(3).

Self-explanation 5. If P and Q have different centroids but the same shape, what does Kabsch give?

Answer. The translation aligning the centroids; the rotation is the shape’s optimal alignment. For identical shapes, R = I, t = q̄ − p̄.

WE-L.2.b — Completion problem

Input. P rotated 90° about Z; Q = P. Find T aligning P to Q.

Trace (final 25% as prompts).

1. Centroids equal (same shape). ⟨verify⟩
2. H after centering gives the covariance of the rotated shape. ⟨verify⟩
3. SVD gives R = Rz(−90°). ⟨verify⟩
4. t = 0. ⟨verify⟩

Answers. All correct.

The lesson. Kabsch recovers the inverse of the applied rotation, not the applied rotation.

WE-L.2.c — Faded: point-to-plane vs. point-to-point

Setup. Two parallel planes, one displaced along the normal by 1 cm, the other displaced along the surface by 1 cm.

Question. What are the residuals?

You must produce the intermediate steps. The setup and the answer are given.

Answer. Normal displacement: point-to-point residual 1 cm, point-to-plane residual 1 cm. Surface displacement: point-to-point residual 1 cm, point-to-plane residual 0 cm (no normal-direction error). Point-to-plane ignores surface sliding; point-to-point does not.

The lesson. In flat regions, point-to-plane converges without drift; point-to-point drifts along the surface.

WE-L.2.d — Contrastive near-miss: initial pose too far

Correct solution. ICP initialized within basin (rotation < 30°).

Near-miss. ICP initialized at 180° rotation.

Why the near-miss produces a plausible but incorrect number. ICP converges to a local minimum at 180° (if the object is symmetric) or fails. The residual is small (symmetry) but the pose is wrong.

The discrepancy. Initial pose far from true; symmetry makes the wrong pose look right. Global registration is required.

Self-explanation 6. How to detect the symmetric local minimum?

Answer. Check ADD-S vs. ADD: ADD-S small, ADD large means a symmetric local minimum. The fix is a hybrid metric or symmetry-breaking prior.

Part C — Global Registration: FPFH, RANSAC, FGR, NDT

§L.3 — Objects: FPFH, RANSAC, FGR, NDT

Global registration finds T from unknown initial pose and unknown correspondences.

FPFH (Fast Point Feature Histograms) (Rusu et al. 2009, tier A, S):

For each point, FPFH encodes the surface’s local geometry:

1. SPFH (Simplified Point Feature Histogram) — for each neighbor q_j, compute the pair (Darboux frame) (α, φ, θ), bin into a histogram.
2. FPFH — the weighted sum of SPFH over the neighbors, giving a 33-dim descriptor.
3. Invariant to rigid transforms; discriminative enough for matching — and discriminative enough is a measurable condition, not a given. The lab’s canary: at 0.8 mm sensor noise a local k-nearest-neighbour descriptor (the FPFH analogue here) moves by 0.377 in descriptor space while the nearest other descriptor is 0.132 away. Retrieval collapses to 0.5%. The descriptor is not “invariant to noise”, and FPFH is not either: it is built from the same local neighbourhood and degrades the same way. What survives is a signature averaged over a much larger neighbourhood (36% retrieval at the same noise), and that is what RANSAC needs.

RANSAC on FPFH matches:

1. Match each point’s FPFH to the nearest in the other cloud (features → candidate correspondences).
2. Sample three correspondences, solve Kabsch.
3. Count inliers (residual < τ).
4. Keep the best-scoring transform.

FGR (Fast Global Registration) (Zhou et al. 2016, tier B, S):

A robust optimization with the Geman-McClure cost, avoiding RANSAC’s sampling. Faster than FPFH + RANSAC at similar accuracy.

NDT (Normal Distributions Transform) (Biber and Strasser 2003, tier B, S):

Voxelize the target, fit a Gaussian per voxel, score by the sum of the source points’ likelihoods in the target voxels. Fast for 2D scan matching; used in SLAM.

Comparison:

§L.3.1 — Result: FPFH is invariant and discriminative; RANSAC is robust; FGR is faster at similar accuracy

Claim. FPFH is invariant under rigid transforms and discriminative enough to identify correspondences across unknown initial pose. RANSAC with three-point samples is robust to outliers. FGR optimizes a robust cost faster than RANSAC.

Proof sketch. FPFH’s bin counts are invariant to SE(3) (computed from pairwise distances and normals). The RANSAC probability 1 − (1 − w³)^K with w the fraction of inliers follows the standard formula. FGR’s Geman-McClure cost gives bounded residuals for outliers, making it robust without sampling.

Corollary. FPFH + RANSAC is the standard for small-medium point clouds; FGR for large clouds; NDT for 2D.

Corollary. Global registration is required when 2.1.2’s pose is unavailable (no model correspondences) or unreliable (symmetric ambiguity).

§L.3.2 — Worked-example ladder

WE-L.3.a — Fully worked: FPFH for a single point

Input. A point p with neighbors {q_1, q_2, q_3} within radius r.

Trace.

1. Compute SPFH(p) from the three pairs’ (α, φ, θ) binned into a 11-dim histogram.
2. For each neighbor q_j, compute SPFH(q_j) from its own neighborhood.
3. FPFH(p) = SPFH(p) + (1/3) Σ_j SPFH(q_j) / d(p, q_j).
4. Normalize to sum 33 (11 bins × 3).

Output and interpretation. FPFH(p) captures both local and neighborhood geometry. Two corresponding points (same physical location on the object) have FPFH distance < 0.1.

Self-explanation 7. Why is FPFH preferred over SPFH alone?

Answer. SPFH uses only the immediate neighbors; FPFH aggregates over two rings of neighbors, giving more discrimination and robustness to noise.

Self-explanation 8. Why 33 dimensions?

Answer. SPFH uses 11 bins per pair (α, φ, θ combined); FPFH weights three SPFHs, giving 33.

WE-L.3.b — Completion problem

Input. 100 points, 30 inliers (30% inlier ratio). How many RANSAC iterations for 99% success?

Trace (final 25% as prompts).

1. w = 0.3, m = 3. ⟨verify⟩
2. P = 1 − (1 − 0.3³)^K = 1 − 0.973^K. ⟨verify⟩
3. For P > 0.99: 0.973^K < 0.01 → K > 168. ⟨verify⟩
4. Choose K = 200 for safety. ⟨verify⟩

Answers. All correct.

The lesson. Inlier ratio matters: 30% needs 200 iterations; 50% needs 70; 10% needs 4600.

WE-L.3.c — Faded: FGR vs. FPFH + RANSAC

Setup. A 100k-point cloud, 20% inliers.

Question. Which algorithm and why?

You must produce the intermediate steps. The setup and the answer are given.

Answer. FGR — faster and robust to outliers. FPFH + RANSAC would need thousands of iterations (20% inliers, m = 3, 99% success → 4580 iterations). FGR’s robust cost handles outliers without sampling.

The lesson. Large clouds with low inlier ratio favor FGR.

WE-L.3.d — Contrastive near-miss: RANSAC without FPFH

Correct solution. FPFH + RANSAC.

Near-miss. RANSAC on raw point coordinates.

Why the near-miss produces a plausible but incorrect number. Raw coordinates are not invariant; two corresponding points in different poses have different coordinates. RANSAC on raw correspondences cannot identify the inliers.

The discrepancy. FPFH gives invariant features; raw coordinates do not.

Self-explanation 9. Why not use color as a feature?

Answer. Color is not always available (LiDAR, depth-only); it is not invariant to lighting; it is not geometric. FPFH is purely geometric and lighting-invariant.

Part D — Deep Learning: PointNet, Learned Registration

§L.4 — Objects: PointNet’s architecture, permutation invariance

PointNet (Qi et al. 2017, tier A, S):

1. Shared MLP — apply the same MLP (e.g., 3 → 64 → 128 → 1024) to each point independently.
2. Max-pooling — take the max over points at each feature dimension, giving a global feature vector.
3. Classification head — apply the MLP to the global feature, output class probabilities.

Permutation invariance comes from max-pooling: max(f(p_1), …, f(p_N)) is invariant to the order of the points.

PointNet++: hierarchical grouping, local features.

Learned registration:

• PointNetLK (Aoki et al. 2019) — PointNet features + Lucas-Kanade alignment.
• DCP (Wang and Solomon 2019) — learned correspondence + Kabsch.
• PCRNet (Sarode et al. 2019) — PointNet features + MLP for pose.
• Foundation models (2023+) — large-scale pretrained point cloud transformers.

Comparison:

§L.4.1 — Result: max-pooling gives permutation invariance; deep registration is fast but requires training

Claim. PointNet’s max-pooling is the symmetry that gives permutation invariance. Deep registration (PointNetLK, DCP) is fast at inference but requires training on the target domain.

Proof sketch. max(f(p_i)) is invariant to the permutation of the p_i, so the network’s output is invariant. Deep registration learns the correspondence/pose mapping from data, replacing the ICP iteration with a single forward pass.

Corollary. Classical ICP is more general (no training) but slower per iteration. Deep registration is faster at inference but domain-specific.

Corollary. The two are combined: deep registration for the initial, classical ICP for the refinement.

§L.4.2 — Worked-example ladder

WE-L.4.a — Fully worked: PointNet forward for one point

Input. p = (0.1, 0.2, 0.3), shared MLP 3 → 64 → 128 → 1024.

Trace.

1. h_1 = MLP_1(p), h_1 ∈ ℝ⁶⁴.
2. h_2 = MLP_2(h_1), h_2 ∈ ℝ¹²⁸.
3. h_3 = MLP_3(h_2), h_3 ∈ ℝ¹⁰²⁴.
4. Global feature g = max_i h_3(p_i) (per dimension over all points).
5. Classification MLP: g → 256 → 40 (40 classes).

Output and interpretation. For a single point, the output is its feature h_3. The global feature aggregates over all points via max-pooling.

Self-explanation 10. Why is max-pooling preferred over mean-pooling?

Answer. Max-pooling is order-invariant and emphasizes the most salient features, giving better discrimination. Mean-pooling is also order-invariant but loses the peaks.

Self-explanation 11. If the point order changes, does the classification change?

Answer. No — the max-pooling is invariant, so the global feature is identical and the classification is unchanged.

WE-L.4.b — Completion problem

Input. 1000 points, PointNet forward.

Trace (final 25% as prompts).

1. Shared MLP per point: 1000 × 3 → 1000 × 64. ⟨verify⟩
2. Down: 1000 × 64 → 1000 × 128 → 1000 × 1024. ⟨verify⟩
3. Max-pool: 1000 × 1024 → 1024. ⟨verify⟩
4. Classify: 1024 → 256 → 40. ⟨verify⟩

Answers. All correct.

The lesson. The per-point MLP is applied independently; the max-pooling reduces over the points.

WE-L.4.c — Faded: PointNetLK for registration

Setup. Source 1000 points, target 1000 points.

Question. How does PointNetLK register?

You must produce the intermediate steps. The setup and the answer are given.

Answer. Extract PointNet features for both; the LK algorithm iteratively solves for T minimizing the feature difference; converges in ~10 iterations. The LK is on the feature space (1024 dims), which is invariant and discriminative.

The lesson. PointNetLK replaces raw point alignment with feature alignment, giving robustness to noise.

WE-L.4.d — Contrastive near-miss: PointNet on unnormalized input

Correct solution. Normalize points to [−1, 1]³ before PointNet.

Near-miss. Raw points in arbitrary units.

Why the near-miss produces a plausible but incorrect number. The MLP’s weights are trained on normalized inputs; raw inputs (in mm) give features 1000× off, and the network fails.

The discrepancy. Input normalization must match training.

Self-explanation 12. Why normalize to [−1, 1]³?

Answer. Keeps activations well-scaled; the network’s weights are tuned for this range.

Part E — Failure Taxonomy, Lab, Cheat Sheet, Exercises, Exit Test

§L.5 — Failure taxonomy

§L.6 — Lab

§L.6.1 — Task

Implement ICP and point-to-plane for the cube’s observed face to the model, verify convergence, implement FPFH + RANSAC for global registration, and break four ways.

§L.6.2 — Conditions

Python 3.10+, CPU. Seed 42. K as above. Cube side 0.05 m. Observed face ~10k points. Model ~15k points. Initial pose from 2.1.2 (small offset) or from FPFH + RANSAC (random).

§L.6.3 — Controls held fixed

Seed, K, the irregular blob model, the sensor-facing 46% subset, 0.8 mm noise, 8 mm correspondence gate. Variants: wrong scale, local minimum, wrong normals (wrong frame, and the sign-convention canary), far initial pose.

Why the object is a blob and not a cube. A cube is invariant under every 90° rotation about a face normal, so “the pose” is only defined up to that symmetry and a pose-error metric cannot tell a correct answer from a rotated one — a 180° break-it on a cube returns 0.00 mm and looks like success. A regular grid on a box is worse: every interior point gets an identical local descriptor, so coarse registration has nothing to match on. Both defects were present in this lab’s first draft.

§L.6.4 — Metrics table

§L.6.5 — Pre-registered hypotheses

• H1. ICP with near initial pose converges in < 20 iterations, residual_mean < 2 mm, pose_error < 1 cm. Prediction: all three. Measured: 34 iterations, residual 4.032 mm, pose error 1.35° / 0.888 mm. Partly refuted — the pose converges, the iteration count and residual do not. The residual is the mean nearest-neighbour distance, which cannot fall below the 0.8 mm sensor noise plus the partial-overlap penalty, so < 2 mm was never a reachable target here.
• H2. Point-to-plane converges in < 5 iterations (5× faster than point-to-point). Prediction: < 5. Measured: 27 iterations against point-to-point’s 34 — a 1.26× speedup, not 5×. Refuted. Point-to-plane is also more accurate here (0.96° / 0.362 mm vs 1.35° / 0.888 mm). The 5× figure is a textbook value for dense, near-complete, low-noise scans; at 46% visibility and 0.8 mm noise the correspondence gate dominates the iteration count, not the convergence order.
• H3. ICP with initial 180° rotation fails (local minimum). Prediction: pose_error > 5 cm. Measured: 174.57° / 7.462 mm, residual 6.617 mm. Met — the pose error is 17° from the nearest symmetric answer and the residual is higher than H1’s, so the failure is visible in the residual as well as the pose.
• H4. Scale mismatch (10× off) diverges. Prediction: residual_mean > 1 m. Measured: no estimate at all — all 1500 correspondences fall outside the 8 mm gate on iteration 1, and the guarded kabsch refuses to run on an empty set. The prediction’s shape was wrong: the failure is not a large residual, it is a refusal to move. An unguarded kabsch returns NaN here, which propagates silently through every later iteration.
• H5. FPFH + RANSAC with 30% inliers succeeds in < 200 iterations. Prediction: pose_error < 2 cm. Measured: 95.0% inliers, 33 ICP iterations, pose error 1.26° / 1.027 mm. Met — but only after the descriptor was replaced. A local k-nearest-neighbour descriptor retrieves the correct correspondence 0.5% of the time at 0.8 mm noise, because the descriptor moves 0.377 while the nearest other descriptor is 0.132 away. The sorted-distance-to-200-neighbours signature retrieves 36%, which RANSAC can use.

§L.6.6 — Break-it sequence

1. Scale mismatch. Multiply observed by 10. Predicted: divergence. Actual: no estimate — every correspondence is rejected on iteration 1. Predicted correctly in outcome, wrong in form: the failure is a refusal to move, and the unguarded version returns NaN rather than a large residual.
2. Wrong initial. 180° rotation. Predicted: local minimum. Actual: local minimum, 174.57° / 7.462 mm. Predicted correctly.
3. Wrong normals. The lesson’s original break-it was “flip every other normal”. That is not a defect at all: negating normal i flips both r_i and J_i, so JᵀJ and Jᵀr are bit-identical — measured, 0.96° / 0.362 mm, exactly H2’s result. Normal sign is a convention. The real defect is normals in the wrong frame (model-frame normals used against world-frame points). Predicted: large pose error. Actual: 1.06° / 1.306 mm after 100 iterations. Refuted — and a further sweep shows the robustness is general: even random unit normals converge to 5.11° / 3.536 mm. Point-to-plane is much less normal-sensitive than the textbook warning implies, because the translation block of J is the normals themselves and the fit averages over them. A break-it built on normal noise needs a scene where the normals disagree about a direction, not merely about their own values.
4. Far initial. 5 m away. Predicted: ICP converges to local minimum. Actual: no estimate — the gate rejects everything on iteration 1. Partly refuted: the predicted outcome (failure) holds, but not the predicted mechanism. There is no local minimum to fall into; there is no correspondence set at all.

Three of four predicted correctly; the wrong-normal break-it was degenerate in its original form and refuted in its repaired form.

§L.6.7 — Acceptance criteria

§L.6.8 — What good looks like

1. ICP’s convergence demonstrated with residual_mean decreasing — and the residual floored at the noise, not driven to zero: the mean nearest-neighbour distance cannot go below the sensor noise plus the partial-overlap penalty, so a “converged to 0.00 mm” claim on a noisy cloud is a sign the residual is being computed between corresponding points rather than nearest neighbours.
2. Point-to-plane’s speed measured, not assumed — 1.26× here, not 5×, and the report says so.
3. The local minimum’s 180° demonstrated at 174.57° with a higher residual than the successful run, so the failure is visible in both channels.
4. Coarse registration shown to be descriptor-limited: the retrieval rate is printed before RANSAC runs, because a descriptor that cannot retrieve makes the inlier count meaningless.
5. The four break-its predicted with symptoms before running, and the predictions scored honestly — including the one that was degenerate as written.

§L.6.9 — Runnable lab code

# lab_2_1_4.py — ICP, point-to-plane, and coarse global registration
import torch
import numpy as np
from scipy.spatial import cKDTree

torch.manual_seed(42)
dtype = torch.float64
MIN_PAIRS = 6          # below this the normal equations are underdetermined

def blob(n=1500, seed=0):
    """A deterministic irregular closed surface: a sphere with harmonic bumps.
    The irregularity is load-bearing. A cube is invariant under every 90 deg rotation
    about a face normal, so 'the' pose is undefined up to that symmetry and the 180 deg
    break-it returns 0.00 mm because the cube really does map onto itself. A regular grid
    on a box is worse still: every interior point gets an identical local descriptor, so
    coarse registration has nothing to match on."""
    r = np.random.default_rng(seed)
    v = r.normal(size=(n, 3))
    v /= np.linalg.norm(v, axis=1, keepdims=True)
    th, ph = np.arccos(v[:,2]), np.arctan2(v[:,1], v[:,0])
    rad = 0.030 * (1 + 0.22*np.sin(3*ph)*np.sin(2*th) + 0.14*np.cos(5*th) + 0.09*np.sin(7*ph))
    p = v * rad[:,None]
    p = p + np.array([0.004, 0.0, 0.0]) * np.sin(4*th)[:,None]
    return torch.tensor(p, dtype=dtype)

def rot(axis, angle):
    a = np.asarray(axis, float); a = a / np.linalg.norm(a)
    K = np.array([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
    return torch.tensor(np.eye(3) + np.sin(angle)*K + (1-np.cos(angle))*(K@K), dtype=dtype)

def kabsch(P, Q):
    """Rigid (R, t) with R P + t = Q. Needs at least 3 non-collinear pairs."""
    pc, qc = P.mean(0), Q.mean(0)
    U, S, Vt = torch.linalg.svd((P - pc).T @ (Q - qc))
    d = torch.det(Vt.T @ U.T)
    R = Vt.T @ torch.diag(torch.tensor([1.0, 1.0, d], dtype=dtype)) @ U.T
    return R, qc - R @ pc

def estimate_normals(pts, k=10):
    tree = cKDTree(pts.numpy()); N = torch.zeros_like(pts)
    for i in range(pts.shape[0]):
        _, idx = tree.query(pts[i].numpy(), k)
        c = pts[idx] - pts[idx].mean(0)
        N[i] = torch.linalg.svd(c.T @ c)[2][-1]
    return N

def icp_point_to_point(P, Q, R0, t0, max_iter=100, tol=1e-10, thresh=None):
    tree = cKDTree(Q.numpy())
    R, t = R0.clone(), t0.clone()
    residuals = []
    for _ in range(max_iter):
        P_t = (R @ P.T).T + t
        d, idx = tree.query(P_t.numpy())
        keep = torch.ones(len(d), dtype=torch.bool) if thresh is None else torch.tensor(d < thresh)
        if int(keep.sum()) < MIN_PAIRS:
            print(f"  [stop] {int(keep.sum())} correspondences within {thresh} m "
                  f"-- the gate rejected everything; kabsch on an empty set returns NaN")
            break
        residuals.append(float(d.mean()))
        R_new, t_new = kabsch(P[keep], Q[idx][keep])
        shift = float((R_new - R).norm()) + float((t_new - t).norm())
        R, t = R_new, t_new
        if shift < tol:
            break
    return R, t, residuals

def icp_point_to_plane(P, Q, N, R0, t0, max_iter=100, tol=1e-12, thresh=None):
    """Minimise sum ((R p + t - q) . n)^2 over correspondences.
    Negating EVERY normal leaves J^T J and J^T r unchanged (both r_i and J_i flip sign),
    so the fit is invariant: normal sign is a convention, not a constraint."""
    tree = cKDTree(Q.numpy())
    R, t = R0.clone(), t0.clone()
    residuals = []
    for _ in range(max_iter):
        P_t = (R @ P.T).T + t
        d, idx = tree.query(P_t.numpy())
        keep = torch.ones(len(d), dtype=torch.bool) if thresh is None else torch.tensor(d < thresh)
        if int(keep.sum()) < MIN_PAIRS:
            print(f"  [stop] {int(keep.sum())} correspondences within {thresh} m")
            break
        Qc, Nc, Pk = Q[idx][keep], N[idx][keep], P_t[keep]
        r = ((Pk - Qc) * Nc).sum(1)
        residuals.append(float(r.abs().mean()))
        J = torch.cat([Nc, torch.linalg.cross(Pk, Nc)], 1)
        delta = torch.linalg.solve(J.T @ J + 1e-9*torch.eye(6, dtype=dtype), -(J.T @ r))
        w = delta[3:]; ang = float(w.norm())
        if ang < 1e-12:
            R_d = torch.eye(3, dtype=dtype)
        else:
            ax = (w / w.norm()).tolist()
            K = torch.tensor([[0,-ax[2],ax[1]],[ax[2],0,-ax[0]],[-ax[1],ax[0],0]], dtype=dtype)
            R_d = torch.eye(3, dtype=dtype) + np.sin(ang)*K + (1-np.cos(ang))*(K@K)
        R = R_d @ R
        t = t + delta[:3]
        if float(delta.norm()) < tol:
            break
    return R, t, residuals

def pose_error(R, t, R_gt, t_gt):
    cos = np.clip((float((R @ R_gt.T).trace()) - 1) / 2, -1.0, 1.0)
    return float(np.degrees(np.arccos(cos))), float((t - t_gt).norm() * 1000)

def describe(pts, nb=200):
    """Rotation- and translation-invariant signature: the sorted distances from each
    point to its nb nearest neighbours. Local descriptors built from the k nearest
    neighbours (k ~ 12) do NOT survive sensor noise here: at 0.8 mm the descriptor
    moves by 0.377 while the nearest other descriptor is 0.132 away, a 3x overload,
    and retrieval falls to chance. Averaging the sorted-distance vector over 200
    neighbours keeps 36% of exact matches at that noise level, which RANSAC can use."""
    d, _ = cKDTree(pts.numpy()).query(pts.numpy(), nb)
    return torch.tensor(d, dtype=dtype)

# --- scene ---
model = blob(1500)
R_gt = rot([0.3, 0.5, 0.8], np.deg2rad(12))
t_gt = torch.tensor([0.010, 0.005, 0.020], dtype=dtype)
SIGMA, THRESH = 0.0008, 0.008

full = (R_gt @ model.T).T + t_gt
full = full + torch.tensor(np.random.default_rng(42).normal(0, SIGMA, full.shape), dtype=dtype)
N_model = estimate_normals(model)
N_world = (R_gt @ N_model.T).T
facing = ((N_world * (-full / full.norm(dim=1, keepdim=True))).sum(1) > 0.15)
observed = full[facing]
N_obs = N_world[facing]
print(f"scene: {model.shape[0]} model pts, {observed.shape[0]} observed "
      f"({100*float(facing.float().mean()):.0f}% visible), noise {SIGMA*1000:.1f} mm")

R0 = rot([0.0, 0.0, 1.0], np.deg2rad(8))
t0 = torch.tensor([0.015, -0.010, 0.030], dtype=dtype)
a0, e0 = pose_error(R0, t0, R_gt, t_gt)
print(f"initial error: {a0:.2f} deg, {e0:.2f} mm")

R_pp, t_pp, res_pp = icp_point_to_point(model, observed, R0, t0, thresh=THRESH)
a_pp, e_pp = pose_error(R_pp, t_pp, R_gt, t_gt)
print(f"H1 point-to-point: {len(res_pp)} iters, residual {res_pp[-1]*1000:.3f} mm, "
      f"pose error {a_pp:.2f} deg / {e_pp:.3f} mm")

R_pl, t_pl, res_pl = icp_point_to_plane(model, observed, N_obs, R0, t0, thresh=THRESH)
a_pl, e_pl = pose_error(R_pl, t_pl, R_gt, t_gt)
print(f"H2 point-to-plane: {len(res_pl)} iters, residual {res_pl[-1]*1000:.3f} mm, "
      f"pose error {a_pl:.2f} deg / {e_pl:.3f} mm")

print("\n--- break-its ---")
R_s, t_s, res_s = icp_point_to_point(model, observed*10, R0, t0, thresh=THRESH)
if res_s:
    print(f"B1 scale x10: {len(res_s)} iters, residual {res_s[-1]:.3f} m, "
          f"pose error {pose_error(R_s, t_s, R_gt, t_gt)[1]:.1f} mm")
else:
    print("B1 scale x10: no estimate -- the gate rejected every correspondence on "
          "iteration 1. The failure is a refusal to move, not a wrong answer.")

R_180 = rot([0.0, 0.0, 1.0], np.pi) @ R0
R_lm, t_lm, res_lm = icp_point_to_point(model, observed, R_180, t0, thresh=THRESH)
a_lm, e_lm = pose_error(R_lm, t_lm, R_gt, t_gt)
print(f"B2 180 deg init: {len(res_lm)} iters, residual {res_lm[-1]*1000:.3f} mm, "
      f"pose error {a_lm:.2f} deg / {e_lm:.3f} mm")

R_wn, t_wn, res_wn = icp_point_to_plane(model, observed, N_model, R0, t0, thresh=THRESH)
a_wn, e_wn = pose_error(R_wn, t_wn, R_gt, t_gt)
print(f"B3 stale-frame normals: {len(res_wn)} iters, residual {res_wn[-1]*1000:.3f} mm, "
      f"pose error {a_wn:.2f} deg / {e_wn:.3f} mm")

R_all, t_all, _ = icp_point_to_plane(model, observed, -N_obs, R0, t0, thresh=THRESH)
a_all, e_all = pose_error(R_all, t_all, R_gt, t_gt)
print(f"B3b all normals negated: {a_all:.2f} deg / {e_all:.3f} mm "
      f"(identical to H2 -- a SIGN CONVENTION, not a defect)")

t_far = t0 + torch.tensor([5.0, 0.0, 0.0], dtype=dtype)
R_far, t_fo, res_far = icp_point_to_point(model, observed, R0, t_far, thresh=THRESH)
if res_far:
    a_far, e_far = pose_error(R_far, t_fo, R_gt, t_gt)
    print(f"B4 far init (5 m): {len(res_far)} iters, residual {res_far[-1]*1000:.1f} mm, "
          f"pose error {a_far:.1f} deg / {e_far:.1f} mm")
else:
    print("B4 far init (5 m): no estimate -- ICP is a LOCAL method; a 5 m offset is "
          "not a refinement problem")

print("\n--- coarse global registration ---")
def coarse_align(P, Q, n_samples=600, thresh=0.006, seed=0):
    """Match by rotation-invariant descriptor, then RANSAC over 3 putative pairs."""
    r = np.random.default_rng(seed)
    DP, DQ = describe(P), describe(Q)
    _, j = cKDTree(DP.numpy()).query(DQ.numpy())
    tree = cKDTree(Q.numpy())
    best = (-1, None, None)
    for _ in range(n_samples):
        s = r.choice(Q.shape[0], 3, replace=False)
        R, t = kabsch(Q[s], P[j[s]])
        d, _ = tree.query(((R @ Q.T).T + t).numpy())
        n_in = int((d < thresh).sum())
        if n_in > best[0]:
            best = (n_in, R, t)
    return best, j

sub = model[::3]
(n_in, R_c, t_c), match = coarse_align(sub, observed)
print(f"RANSAC: {n_in}/{sub.shape[0]} inliers ({100*n_in/sub.shape[0]:.1f}%)")
R_g, t_g, res_g = icp_point_to_point(model, observed, R_c, t_c, thresh=THRESH)
a_g, e_g = pose_error(R_g, t_g, R_gt, t_gt)
print(f"H5 coarse + ICP: {len(res_g)} iters, pose error {a_g:.2f} deg / {e_g:.3f} mm")

print("\n--- lab 2.1.4 summary ---")
print(f"H1: point-to-point {len(res_pp)} iters, {a_pp:.2f} deg / {e_pp:.3f} mm")
print(f"H2: point-to-plane {len(res_pl)} iters, {a_pl:.2f} deg / {e_pl:.3f} mm")
print(f"H3: 180 deg init -> {a_lm:.2f} deg / {e_lm:.3f} mm")
print(f"H4: scale x10 -> {'no estimate (gate rejected all)' if not res_s else f'{res_s[-1]:.3f} m'}")
print(f"H5: coarse + ICP -> {a_g:.2f} deg / {e_g:.3f} mm")
print(f"B3: stale normals -> {a_wn:.2f} deg / {e_wn:.3f} mm")

§L.7 — The cheat sheet

§L.8 — Exercises with solutions

E 2.1.4.1 — Back-projection (numerical). Pixel (500, 200), Z = 1.5, K = [[500, 0, 320], [0, 500, 240], [0, 0, 1]].

S 2.1.4.1. X = (500 − 320) × 1.5 / 500 = 0.54. Y = (200 − 240) × 1.5 / 500 = −0.12. Z = 1.5. The lesson: positive X, negative Y — right of center, above center.

E 2.1.4.2 — Kabsch (numerical). P = {(0,0,0), (1,0,0), (0,1,0)}, Q = {(1,1,1), (2,1,1), (1,2,1)}.

S 2.1.4.2. t = (1, 1, 1), R = I. The lesson: pure translation.

E 2.1.4.3 — Point-to-plane (numerical). Two parallel planes, displacement 1 cm along normal.

S 2.1.4.3. Point-to-plane residual 1 cm; point-to-point residual 1 cm. Normal displacement: both detect. The lesson: normal displacement is detected by both; surface sliding is penalized only by point-to-point.

E 2.1.4.4 — RANSAC iterations (numerical). Inlier 20%, m = 3, success 99%.

S 2.1.4.4. P = 1 − (1 − 0.008)^K = 0.99 → K ≈ 573. The lesson: low inlier ratio needs many iterations.

E 2.1.4.5 — FPFH invariance (conceptual). Two points on the same physical surface at different poses.

S 2.1.4.5. Same FPFH because FPFH is computed from local geometry, invariant to rigid transforms. The lesson: invariance gives matching across poses.

E 2.1.4.6 — Local minimum diagnosis (diagnostic). ICP converges, ADD-S = 0.001, ADD = 0.04.

S 2.1.4.6. Symmetric local minimum. The lesson: ADD vs. ADD-S distinguishes.

E 2.1.4.7 — PointNet (conceptual). Why is max-pooling chosen over mean-pooling?

S 2.1.4.7. Both are permutation-invariant; max emphasizes salient features. The lesson: max is more discriminative.

E 2.1.4.8 — FGR vs. RANSAC (constructive). Given 100k points, 15% inliers. Algorithm?

S 2.1.4.8. FGR: faster, robust. The lesson: large clouds with low inlier ratio favor FGR.

§L.9 — Exit test

You are ready for Lesson 2.1.5 (SLAM and Visual Odometry) when you can, without notes:

1. State what a point cloud is and its properties (permutation-invariance, no connectivity).
2. Convert a depth image to a point cloud.
3. State the ICP’s steps and Kabsch’s closed form.
4. State the point-to-plane’s 5–10× speedup in flat regions.
5. State the ICP’s local basin (30°, 10%).
6. State FPFH, RANSAC, FGR, and their trade-offs.
7. State the PointNet’s max-pooling and the permutation invariance.
8. Diagnose wrong correspondence (residual large), local minimum (ADD small, ADD-S big), scale mismatch (the correspondence gate rejects everything — no estimate, not a large residual), wrong normals (normals in the wrong frame; note that negating every normal changes nothing, and point-to-plane survives even random normals to a few degrees).
9. State the units consistency requirement.
10. State the global registration’s need when the initial is far.
11. State the combined pipeline (FPFH + RANSAC, then ICP).
12. State the PointNetLK, DCP, PCRNet and their speed advantage over classical.

If any is not answerable, return to: 1 to §L.1, 2 to §L.1, 3 to §L.2, 4 to §L.2, 5 to §L.2.1, 6 to §L.3, 7 to §L.4, 8 to §L.5, 9 to §L.2, 10 to §L.3, 11 to §L.3, 12 to §L.4.

End of Lesson 2.1.4. The learner has now completed point clouds and registration: back-projection, ICP (point-to-point and point-to-plane), Kabsch, FPFH and RANSAC, FGR, and PointNet’s permutation invariance. The next lesson, 2.1.5 — Visual SLAM and Odometry, extends the registration across time to estimate the camera’s trajectory and the map — the perception’s back-end for the 2.4.1’s SLAM.