#!/usr/bin/env python3 """ psp_walkthrough.py ================== Step-by-step demonstration of phase-shift profilometry (PSP / PMP), using the same geometry as ftp_walkthrough.py (1 mm fringe period, L = 500 mm, d = 150 mm) so the numbers can be compared directly. Figure 1 psp_1_pipeline.png The per-pixel solve: four phase-shifted frames, the four samples at two pixels with the fitted cosine, the A (texture) and B (modulation) maps, wrapped phase, height, error map, and a row profile compared with FTP on the same data. Figure 2 psp_2_gamma_aliasing.png Why N-step PSP rejects harmonics up to order N-2: harmonic content of a gamma-distorted fringe, the aliasing table (harmonic k leaks into the phase if k = +/-1 mod N), the phase-error ripple vs phase for N = 3, 4, 8, and RMS error vs gamma for N = 3..8. Figure 3 psp_3_heterodyne_unwrapping.png Temporal (heterodyne) unwrapping with periods 16, 18, 20 px recovers the 2.2 mm step that spatial unwrapping gets wrong by one fringe. Also: fraction of pixels given the wrong fringe order vs camera noise. Model: the object shifts the projector coordinate seen by each camera pixel by du = h * k * P0 / (2*pi) pixels (small-height approximation), where k is the phase-height sensitivity at the reference period P0. The phase of a pattern with period P is then 2*pi*(x + du)/P, so every period sees the same physical shift. Requires numpy, scipy, matplotlib. Uses scikit-image's 2-D unwrapper if installed. """ from __future__ import annotations import argparse import os from dataclasses import dataclass import numpy as np import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt from scipy.ndimage import gaussian_filter try: from skimage.restoration import unwrap_phase as _sk_unwrap except Exception: _sk_unwrap = None TWO_PI = 2 * np.pi def wrap(p): return np.angle(np.exp(1j * p)) # ---------------------------------------------------------------------------------- # Setup and image formation # ---------------------------------------------------------------------------------- @dataclass class Setup: n: int = 512 P0: float = 16.0 # working fringe period (px) = 1 mm pixel_mm: float = 0.0625 L_mm: float = 500.0 d_mm: float = 150.0 gamma: float = 1.0 # residual projector gamma after any LUT (1 = linear) u0: float = 24.0 # projector-coordinate offset of camera column 0 (px); keeps the # absolute phase away from the 0/2pi wrap at the field edge noise: float = 0.004 # camera noise, fraction of full scale seed: int = 3 @property def k(self): # rad per mm at period P0 return TWO_PI * self.d_mm / (self.L_mm * self.P0 * self.pixel_mm) def du_px(self, h_mm): # projector-coordinate shift caused by height return h_mm * self.k * self.P0 / TWO_PI def xy_mm(self): y, x = np.mgrid[0:self.n, 0:self.n] return x * self.pixel_mm, y * self.pixel_mm def capture(h_mm, R, s: Setup, rng, delta=0.0, P=None): P = s.P0 if P is None else P x = np.arange(s.n)[None, :] t = 0.5 + 0.5 * np.cos(TWO_PI * (x + s.u0 + s.du_px(h_mm)) / P + delta) I = 0.03 + 0.85 * R * t ** s.gamma return np.clip(I + rng.normal(0, s.noise, I.shape), 0, 1) def deltas_for(N): return [TWO_PI * n / N for n in range(N)] def capture_set(h, R, s, rng, N, P=None): return [capture(h, R, s, rng, d, P) for d in deltas_for(N)] # ---------------------------------------------------------------------------------- # PSP core # ---------------------------------------------------------------------------------- def psp(frames, deltas): """Least-squares N-step solve. Returns wrapped phase, A (mean), B (modulation).""" s = sum(I * np.sin(d) for I, d in zip(frames, deltas)) c = sum(I * np.cos(d) for I, d in zip(frames, deltas)) N = len(frames) return np.arctan2(-s, c), np.mean(frames, axis=0), 2.0 / N * np.hypot(s, c) def unwrap2d(p): if _sk_unwrap is not None: return _sk_unwrap(p) u = np.unwrap(p, axis=1) col = np.unwrap(u[:, u.shape[1] // 2]) return u + (col - u[:, u.shape[1] // 2])[:, None] def ftp_phase(img, P, half_width=0.5): """Plain FTP on one frame (for comparison).""" n = img.shape[1] f0 = 1.0 / P f = np.fft.fftfreq(n) u = (f - f0) / (half_width * f0) H = np.where(np.abs(u) < 1, 0.5 * (1 + np.cos(np.pi * u)), 0.0) return np.fft.ifft(np.fft.fft(img, axis=1) * H[None, :], axis=1) def heterodyne(phi_a, phi_b, Pa, Pb): """Beat two wrapped phases (Pa < Pb). Returns phase in [0, 2pi) with period Peq.""" return np.mod(phi_a - phi_b, TWO_PI), Pa * Pb / (Pb - Pa) def unwrap_with(phi_fine, P_fine, Phi_coarse_abs, P_coarse): """Use an absolute coarse phase to choose the fringe order of a wrapped fine phase.""" k = np.round((Phi_coarse_abs * P_coarse / P_fine - phi_fine) / TWO_PI) return phi_fine + TWO_PI * k, k def three_period_absolute(phis, Ps): """Hierarchical heterodyne with three periods P1 < P2 < P3. phi12 (P12) and phi23 (P23) beat again to phi123 (P123), which must exceed the field.""" (p1, p2, p3), (P1, P2, P3) = phis, Ps phi12, P12 = heterodyne(p1, p2, P1, P2) phi23, P23 = heterodyne(p2, p3, P2, P3) phi123, P123 = heterodyne(phi12, phi23, P12, P23) if P12 < P23 else heterodyne(phi23, phi12, P23, P12) Phi12, _ = unwrap_with(phi12, P12, phi123, P123) # phi123 is already absolute Phi1, k1 = unwrap_with(p1, P1, Phi12, P12) return dict(phi12=phi12, phi23=phi23, phi123=phi123, P12=P12, P23=P23, P123=P123, Phi12=Phi12, Phi1=Phi1, k1=k1) # ---------------------------------------------------------------------------------- # Scenes (same as the FTP walkthrough) # ---------------------------------------------------------------------------------- def scene_bumps(s: Setup): x, y = s.xy_mm() c = s.n * s.pixel_mm / 2 h = (2.5 * np.exp(-((x - c + 6) ** 2 + (y - c) ** 2) / (2 * 4.5 ** 2)) + 1.2 * np.exp(-((x - c - 7) ** 2 + (y - c + 5) ** 2) / (2 * 3.0 ** 2)) - 0.8 * np.exp(-((x - c - 4) ** 2 + (y - c - 8) ** 2) / (2 * 3.0 ** 2))) R = np.full_like(h, 0.9) mark = ((x - c - 7) ** 2 / 3.0 ** 2 + (y - c - 9) ** 2 / 1.2 ** 2) < 1 return h, gaussian_filter(np.where(mark, 0.3, R), 1.0) def scene_step(s: Setup, step_mm): x, y = s.xy_mm() c = s.n * s.pixel_mm / 2 box = (np.abs(x - c) < 7) & (np.abs(y - c) < 9) return np.where(box, step_mm, 0.0), np.full((s.n, s.n), 0.9) # ---------------------------------------------------------------------------------- # Figure 1: per-pixel solve # ---------------------------------------------------------------------------------- def fig_pipeline(s: Setup, out): rng = np.random.default_rng(s.seed) h, R = scene_bumps(s) Rref = np.full_like(h, 0.9) N = 4 dl = deltas_for(N) fo = capture_set(h, R, s, rng, N) fr = capture_set(np.zeros_like(h), Rref, s, rng, N) po, A, B = psp(fo, dl) pr, _, _ = psp(fr, dl) wrapped = wrap(po - pr) m = 24 hp = unwrap2d(wrapped) / s.k hp -= np.median((hp - h)[m:-m, m:-m]) err = (hp - h) * 1000 rms = np.sqrt(np.mean(err[m:-m, m:-m] ** 2)) # FTP on frame 0 of the same data ao, ar = ftp_phase(fo[0], s.P0), ftp_phase(fr[0], s.P0) hf = unwrap2d(np.angle(ao * np.conj(ar))) / s.k hf -= np.median((hf - h)[m:-m, m:-m]) errf = (hf - h) * 1000 rmsf = np.sqrt(np.mean(errf[m:-m, m:-m] ** 2)) ext = [0, s.n * s.pixel_mm, s.n * s.pixel_mm, 0] fig, axs = plt.subplots(2, 4, figsize=(18, 8.8)) # (1) 2x2 montage of a crop of the four frames cy, cx, w = 230, 120, 64 crop = [f[cy:cy + w, cx:cx + w] for f in fo] mont = np.block([[crop[0], crop[1]], [crop[2], crop[3]]]) ax = axs[0, 0] ax.imshow(mont, cmap="gray") for (yy, xx), lab in zip([(4, 4), (4, w + 4), (w + 4, 4), (w + 4, w + 4)], ["δ = 0", "δ = π/2", "δ = π", "δ = 3π/2"]): ax.text(xx, yy + 8, lab, color="yellow", fontsize=9, weight="bold") ax.axhline(w - 0.5, color="w", lw=1) ax.axvline(w - 0.5, color="w", lw=1) ax.set_xticks([]) ax.set_yticks([]) ax.set_title("① four frames, fringes shifted by π/2\n(4 mm crop on the big bump)") # (2) samples at two pixels + fitted cosine ax = axs[0, 1] xm_, ym_ = s.xy_mm() c = s.n * s.pixel_mm / 2 pix = [(256, 160, "bright pixel (R = 0.9)", "C0"), (int((c + 9) / s.pixel_mm), int((c + 7) / s.pixel_mm), "pixel in dark mark (R = 0.3)", "C3")] tt = np.linspace(0, TWO_PI, 200) for yy, xx, lab, col in pix: samples = [f[yy, xx] for f in fo] ax.plot(dl, samples, "o", color=col, ms=8) ax.plot(tt, A[yy, xx] + B[yy, xx] * np.cos(po[yy, xx] + tt), color=col, lw=1.3, label=f"{lab}: A={A[yy, xx]:.2f}, B={B[yy, xx]:.2f}, φ={po[yy, xx]:+.2f}") ax.set_xticks([0, np.pi / 2, np.pi, 3 * np.pi / 2, TWO_PI]) ax.set_xticklabels(["0", "π/2", "π", "3π/2", "2π"]) ax.set_xlabel("phase shift δ") ax.set_ylabel("camera intensity") ax.set_ylim(0, 1.05) ax.legend(fontsize=7.5, loc="upper center") ax.set_title("② per pixel: fit A + B·cos(φ + δ) to N samples\n(no neighbours used)") ax = axs[0, 2] im = ax.imshow(A, cmap="gray", extent=ext) ax.set_title("③ A = mean of frames\n(texture image, fringes gone)") fig.colorbar(im, ax=ax, shrink=0.8) ax = axs[0, 3] im = ax.imshow(B / A, cmap="viridis", extent=ext, vmin=0, vmax=1) ax.set_title("④ modulation B/A\n(quality mask: low = shadow, dark, saturated)") fig.colorbar(im, ax=ax, shrink=0.8) ax = axs[1, 0] im = ax.imshow(wrapped, cmap="twilight", vmin=-np.pi, vmax=np.pi, extent=ext) ax.set_title("⑤ wrapped Δφ = φ_obj − φ_ref") fig.colorbar(im, ax=ax, shrink=0.8, label="rad") ax = axs[1, 1] im = ax.imshow(hp, cmap="viridis", extent=ext) ax.set_title(f"⑥ unwrapped → height (mm)\nk = {s.k:.3f} rad/mm") fig.colorbar(im, ax=ax, shrink=0.8, label="mm") ax = axs[1, 2] v = np.percentile(np.abs(errf[m:-m, m:-m]), 99) im = ax.imshow(err, cmap="RdBu_r", vmin=-v, vmax=v, extent=ext) ax.set_title(f"PSP4 height error, RMS {rms:.1f} µm\n(same colour scale as FTP; noise only, larger in the dark mark)") fig.colorbar(im, ax=ax, shrink=0.8, label="µm") ax = axs[1, 3] row = s.n // 2 xm = np.arange(s.n) * s.pixel_mm ax.plot(xm, errf[row], "C1", lw=1, label=f"FTP, frame 0 only (RMS {rmsf:.1f} µm)") ax.plot(xm, err[row], "C0", lw=1, label=f"PSP4, 4 frames (RMS {rms:.1f} µm)") ax.set_xlabel("x (mm)") ax.set_ylabel("height error (µm)") ax.set_title("error along row y = 16 mm, same data") ax.legend(fontsize=8) ax.grid(alpha=0.3) for a in [axs[0, 2], axs[0, 3], axs[1, 0], axs[1, 1], axs[1, 2]]: a.set_xlabel("x (mm)") a.set_ylabel("y (mm)") fig.suptitle("Phase-shift profilometry, step by step — 4-step, period 16 px = 1 mm, " "L = 500 mm, d = 150 mm, linear projector", fontsize=13) fig.tight_layout() fig.savefig(out, dpi=130) plt.close(fig) return rms, rmsf # ---------------------------------------------------------------------------------- # Figure 2: gamma harmonics and aliasing # ---------------------------------------------------------------------------------- def fringe(theta, gamma): return (0.5 + 0.5 * np.cos(theta)) ** gamma def phase_error_curve(N, gamma, nphi=720): """Noise-free single-pixel phase error vs true phase.""" phi = np.linspace(-np.pi, np.pi, nphi, endpoint=False) dl = deltas_for(N) frames = [fringe(phi + d, gamma) for d in dl] est, _, _ = psp(frames, dl) return phi, wrap(est - phi) def fig_gamma(out, gamma=2.2): fig, axs = plt.subplots(1, 4, figsize=(19, 4.6), gridspec_kw=dict(width_ratios=[1, 1.05, 1.2, 1.2])) # (a) harmonic content th = np.linspace(0, TWO_PI, 4096, endpoint=False) kmax = 9 ax = axs[0] for gm, col, off in [(1.0, "C7", -0.2), (2.0, "C2", 0.0), (gamma, "C3", 0.2)]: F = np.abs(np.fft.rfft(fringe(th, gm))) / len(th) * 2 amps = F[1:kmax + 1] / F[1] ax.bar(np.arange(1, kmax + 1) + off, amps, width=0.2, color=col, label=f"γ = {gm:g}") ax.set_yscale("log") ax.set_ylim(1e-6, 2) ax.set_xticks(range(1, kmax + 1)) ax.set_xlabel("harmonic order k") ax.set_ylabel("amplitude relative to k = 1") ax.set_title("(a) harmonics of a gamma-distorted fringe\nγ = 2 has only k = 2 (exactly)") ax.legend(fontsize=8) ax.grid(alpha=0.3, axis="y", which="both") # (b) aliasing table Ns = [3, 4, 5, 6, 8] ks = np.arange(2, 10) tbl = np.array([[1.0 if (k % N in (1, N - 1)) else 0.0 for k in ks] for N in Ns]) ax = axs[1] ax.imshow(tbl, cmap="Reds", vmin=0, vmax=1.6, aspect="auto") for i, N in enumerate(Ns): for j, k in enumerate(ks): ax.text(j, i, "leaks" if tbl[i, j] else "·", ha="center", va="center", fontsize=8, color="k" if tbl[i, j] else "0.6") ax.set_xticks(range(len(ks))) ax.set_xticklabels(ks) ax.set_yticks(range(len(Ns))) ax.set_yticklabels([f"N = {N}" for N in Ns]) ax.set_xlabel("harmonic order k") ax.set_title("(b) harmonic k corrupts φ if k ≡ ±1 (mod N)\nfirst leak is k = N − 1") # (c) error vs phase ax = axs[2] for N, col in [(3, "C3"), (4, "C1"), (8, "C0")]: phi, e = phase_error_curve(N, gamma) ax.plot(phi, np.rad2deg(e), col, lw=1.4, label=f"N = {N}: ripple at {N}φ, peak {np.rad2deg(np.abs(e)).max():.2g}°") ax.set_xlabel("true phase φ (rad)") ax.set_ylabel("phase error (deg)") ax.set_yscale("symlog", linthresh=1e-3) ax.set_title(f"(c) error vs phase, γ = {gamma}, noise-free\n(harmonic k → ripple at (k ± 1)φ = Nφ)") ax.legend(fontsize=8) ax.grid(alpha=0.3) # (d) RMS vs gamma ax = axs[3] gms = np.linspace(1.0, 3.0, 41) for N in [3, 4, 5, 6, 8]: r = [np.sqrt(np.mean(phase_error_curve(N, g)[1] ** 2)) for g in gms] ax.semilogy(gms, np.maximum(r, 1e-9) * 1000, label=f"N = {N}") ax.set_xlabel("projector gamma (no correction)") ax.set_ylabel("RMS phase error (mrad)") ax.set_ylim(1e-4, 500) ax.set_title("(d) RMS phase error vs gamma\n(1 mrad = 0.53 µm here)") ax.legend(fontsize=8) ax.grid(alpha=0.3, which="both") fig.tight_layout() fig.savefig(out, dpi=130) plt.close(fig) # ---------------------------------------------------------------------------------- # Figure 3: heterodyne temporal unwrapping # ---------------------------------------------------------------------------------- def measure_abs(h, R, s, rng, Ps, N=4): """Absolute phase at P0 via three-period heterodyne; also the wrapped P0 phase.""" phis = [psp(capture_set(h, R, s, rng, N, P), deltas_for(N))[0] for P in Ps] return three_period_absolute(phis, Ps), phis def fig_heterodyne(s: Setup, out, Ps=(16.0, 18.0, 20.0), step=2.2): m = 24 h, R = scene_step(s, step) rng = np.random.default_rng(s.seed) ho, phis_o = measure_abs(h, R, s, rng, Ps) hr, _ = measure_abs(np.zeros_like(h), R, s, rng, Ps) h_het = (ho["Phi1"] - hr["Phi1"]) / s.k # spatial unwrapping of the single-period PSP phase, for comparison h_sp = unwrap2d(wrap(phis_o[0] - psp(capture_set(np.zeros_like(h), R, s, rng, 4), deltas_for(4))[0])) / s.k h_sp -= np.median(h_sp[m:-m, m:m + 40]) row = s.n // 2 xpx = np.arange(s.n) xm = xpx * s.pixel_mm fig, axs = plt.subplots(2, 3, figsize=(17, 8.8)) ax = axs[0, 0] for p, P, col in zip(phis_o, Ps, ["C0", "C1", "C2"]): ax.plot(xm, np.mod(p[row], TWO_PI), col, lw=0.9, label=f"P = {P:g} px") ax.set_xlim(0, 8) ax.set_xlabel("x (mm) [first 8 mm shown]") ax.set_ylabel("wrapped phase (rad)") ax.set_title("(a) three wrapped phases from three 4-step sets\n(12 frames total)") ax.legend(fontsize=8) ax.grid(alpha=0.3) ax = axs[0, 1] ax.plot(xm, ho["phi12"][row], "C4", lw=1, label=f"φ₁₂ = φ₁₆ − φ₁₈, P_eq = {ho['P12']:.0f} px") ax.plot(xm, ho["phi23"][row], "C5", lw=1, label=f"φ₂₃ = φ₁₈ − φ₂₀, P_eq = {ho['P23']:.0f} px") ax.plot(xm, ho["phi123"][row], "k", lw=1.6, label=f"φ₁₂₃ = φ₁₂ − φ₂₃, P_eq = {ho['P123']:.0f} px > field ({s.n} px)") ax.set_xlabel("x (mm)") ax.set_ylabel("beat phase (rad)") ax.set_title("(b) beating gives long equivalent periods\nφ₁₂₃ never wraps across the field → absolute") ax.legend(fontsize=7.5, loc="upper left") ax.set_ylim(-0.3, 8.5) ax.grid(alpha=0.3) ax = axs[0, 2] ax.plot(xm, h[row], "k", lw=2.2, label=f"true ({step} mm step = {s.k * step:.2f} rad)") ax.plot(xm, h_sp[row], "C3", lw=1.1, label="single period + spatial unwrap") ax.plot(xm, h_het[row], "C0", lw=1.1, ls="--", label="heterodyne temporal unwrap") ax.set_xlabel("x (mm)") ax.set_ylabel("height (mm)") ax.set_title("(c) the step spatial unwrapping gets wrong,\nrecovered by temporal unwrapping") ax.set_ylim(-1.6, 3.4) ax.legend(fontsize=8, loc="upper right") ax.grid(alpha=0.3) ext = [0, s.n * s.pixel_mm, s.n * s.pixel_mm, 0] ax = axs[1, 0] im = ax.imshow(h_sp, cmap="viridis", extent=ext, vmin=-1.2, vmax=2.4) ax.set_title("(d) spatial unwrap: box 3.33 mm too low\n(one fringe of height)") fig.colorbar(im, ax=ax, shrink=0.8, label="mm") ax = axs[1, 1] im = ax.imshow(h_het, cmap="viridis", extent=ext, vmin=-1.2, vmax=2.4) e = (h_het - h)[m:-m, m:-m] * 1000 ax.set_title(f"(e) heterodyne: correct everywhere\nRMS error {np.sqrt(np.mean(e ** 2)):.1f} µm, " f"wrong orders {np.mean(np.abs(e) > 1000) * 100:.2f} %") fig.colorbar(im, ax=ax, shrink=0.8, label="mm") for a in [axs[1, 0], axs[1, 1]]: a.set_xlabel("x (mm)") a.set_ylabel("y (mm)") # (f) wrong-order fraction vs noise ax = axs[1, 2] sm = Setup(**{**s.__dict__, "n": 192}) hs, Rs = scene_step(sm, step) noises = [0.004, 0.01, 0.02, 0.03, 0.045, 0.06, 0.08] for Pset, lab, col in [((16.0, 18.0, 20.0), "16 / 18 / 20 px (P₁₂₃ = 720)", "C0"), ((16.0, 17.0, 18.0), "16 / 17 / 18 px (P₁₂₃ = 2448)", "C3")]: frac = [] for nz in noises: sn = Setup(**{**sm.__dict__, "noise": nz}) r = np.random.default_rng(11) a, _ = measure_abs(hs, Rs, sn, r, Pset) b, _ = measure_abs(np.zeros_like(hs), Rs, sn, r, Pset) hh = (a["Phi1"] - b["Phi1"]) / sn.k frac.append(max(np.mean(np.abs(hh - hs) > 0.5 * TWO_PI / sn.k) * 100, 1e-3)) ax.semilogy(np.array(noises) * 100, frac, "o-", color=col, label=lab) ax.set_xlabel("camera noise (% of full scale)") ax.set_ylabel("pixels with wrong fringe order (%)") ax.set_ylim(5e-4, 100) ax.set_title("(f) closer periods → longer P_eq but more noise gain\nratio per stage limits robustness") ax.legend(fontsize=8) ax.grid(alpha=0.3, which="both") fig.tight_layout() fig.savefig(out, dpi=130) plt.close(fig) return ho def main(): ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) ap.add_argument("--outdir", default="psp_walkthrough_results") ap.add_argument("--gamma", type=float, default=2.2, help="gamma used in figure 2 (c)") args = ap.parse_args() os.makedirs(args.outdir, exist_ok=True) s = Setup() print(f"k = {s.k:.3f} rad/mm, one fringe of height = {TWO_PI / s.k:.2f} mm, " f"unwrapper: {'scikit-image' if _sk_unwrap else 'fallback'}") rms, rmsf = fig_pipeline(s, os.path.join(args.outdir, "psp_1_pipeline.png")) print(f"fig 1: PSP4 RMS {rms:.1f} um vs FTP {rmsf:.1f} um on the same data") fig_gamma(os.path.join(args.outdir, "psp_2_gamma_aliasing.png"), args.gamma) print("fig 2 done") ho = fig_heterodyne(s, os.path.join(args.outdir, "psp_3_heterodyne_unwrapping.png")) print(f"fig 3: P12 = {ho['P12']:.0f}, P23 = {ho['P23']:.0f}, P123 = {ho['P123']:.0f} px") print(f"figures written to {os.path.abspath(args.outdir)}") if __name__ == "__main__": main()