From bda76ccff2da7c0946a54d239d9c21d2b64ff97d Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 2 Oct 2026 09:19:18 +0000 Subject: [PATCH] =?UTF-8?q?feat(motion):=20add=20FFT=20plans=20and=20Kugli?= =?UTF-8?q?n=E2=80=93Hines=20phase=20correlation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Radix-2 Cooley–Tukey with SIMD butterflies and Bluestein chirp-z for arbitrary lengths, cached plans, parallel 2D transforms, real-input half-length transforms and Tukey/Hann windows. Phase correlation packs both patches into one complex FFT, normalises the cross-power spectrum with a Gaussian low-pass and refines the peak with a log-parabola fit. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_012iLMBNwBE2GnphAsu4cjaC --- src/Titano.Core/Motion/FftPlan.cs | 276 ++++++++++++++++++ src/Titano.Core/Motion/Fourier.cs | 269 +++++++++++++++++ src/Titano.Core/Motion/PhaseCorrelator.cs | 273 +++++++++++++++++ .../Titano.Core.Tests/Motion/FourierTests.cs | 176 +++++++++++ .../Motion/PhaseCorrelatorTests.cs | 83 ++++++ .../Motion/SyntheticImages.cs | 122 ++++++++ 6 files changed, 1199 insertions(+) create mode 100644 src/Titano.Core/Motion/FftPlan.cs create mode 100644 src/Titano.Core/Motion/Fourier.cs create mode 100644 src/Titano.Core/Motion/PhaseCorrelator.cs create mode 100644 tests/Titano.Core.Tests/Motion/FourierTests.cs create mode 100644 tests/Titano.Core.Tests/Motion/PhaseCorrelatorTests.cs create mode 100644 tests/Titano.Core.Tests/Motion/SyntheticImages.cs diff --git a/src/Titano.Core/Motion/FftPlan.cs b/src/Titano.Core/Motion/FftPlan.cs new file mode 100644 index 00000000..412da884 --- /dev/null +++ b/src/Titano.Core/Motion/FftPlan.cs @@ -0,0 +1,276 @@ +using System.Buffers; +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace Titano.Core.Motion; + +/// +/// Precomputed complex DFT of one length on split real/imaginary arrays: iterative radix-2 +/// Cooley–Tukey (1965) for powers of two, Bluestein (1970) chirp-z convolution otherwise. +/// Forward uses e^(−2πikn/N); is normalised by 1/N. +/// Immutable and thread-safe; obtain shared instances from . +/// +public sealed class FftPlan +{ + private readonly int[]? _bitReverse; + // Stage-major twiddles: the stage with half-size h occupies [h − 1, 2h − 1), so SIMD loads are contiguous. + private readonly double[]? _twiddleRe, _twiddleIm; + private readonly FftPlan? _convolution; + private readonly double[]? _chirpRe, _chirpIm; + private readonly double[]? _kernelRe, _kernelIm; + + internal FftPlan(int length) + { + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(length); + Length = length; + IsPowerOfTwo = BitOperations.IsPow2(length); + + // e^(−iπk/N), k = 0..N: the post-processing twiddles of a real transform of length 2N. + HalfTwiddleRe = new double[length + 1]; + HalfTwiddleIm = new double[length + 1]; + for (var k = 0; k <= length; k++) + { + var (s, c) = double.SinCos(double.Pi * k / length); + HalfTwiddleRe[k] = c; + HalfTwiddleIm[k] = -s; + } + + if (IsPowerOfTwo) + { + var bits = BitOperations.Log2((uint)length); + _bitReverse = new int[length]; + for (var i = 0; i < length; i++) + _bitReverse[i] = bits == 0 ? 0 : (int)(ReverseBits((uint)i) >> (32 - bits)); + _twiddleRe = new double[int.Max(1, length - 1)]; + _twiddleIm = new double[_twiddleRe.Length]; + for (var h = 1; h < length; h <<= 1) + { + for (var k = 0; k < h; k++) + { + var (s, c) = double.SinCos(double.Pi * k / h); + _twiddleRe[h - 1 + k] = c; + _twiddleIm[h - 1 + k] = -s; + } + } + return; + } + + // Bluestein: X_k = c_k · Σ_n (x_n c_n) · conj(c_(k−n)), c_k = e^(−iπk²/N), as a power-of-two circular convolution. + var m = (int)BitOperations.RoundUpToPowerOf2((uint)(2 * length - 1)); + _convolution = Fourier.Plan(m); + ScratchLength = 2 * m; + _chirpRe = new double[length]; + _chirpIm = new double[length]; + _kernelRe = new double[m]; + _kernelIm = new double[m]; + for (var k = 0; k < length; k++) + { + // k² mod 2N keeps the angle argument small and exact for large N. + var q = (long)k * k % (2L * length); + var (s, c) = double.SinCos(double.Pi * q / length); + _chirpRe[k] = c; + _chirpIm[k] = -s; + _kernelRe[k] = c; + _kernelIm[k] = s; + if (k > 0) + { + _kernelRe[m - k] = c; + _kernelIm[m - k] = s; + } + } + _convolution.Radix2(_kernelRe, _kernelIm); + var scale = 1.0 / m; + for (var i = 0; i < m; i++) + { + _kernelRe[i] *= scale; + _kernelIm[i] *= scale; + } + } + + /// Transform length N. + public int Length { get; } + + /// True when the radix-2 path is used. + public bool IsPowerOfTwo { get; } + + /// Doubles of scratch the scratch-taking overloads need (0 for powers of two). + public int ScratchLength { get; } + + internal double[] HalfTwiddleRe { get; } + + internal double[] HalfTwiddleIm { get; } + + /// In-place forward DFT. Bluestein lengths rent their scratch from the shared pool. + public void Forward(Span re, Span im) + { + if (ScratchLength == 0) + { + Forward(re, im, Span.Empty); + return; + } + var scratch = ArrayPool.Shared.Rent(ScratchLength); + try + { + Forward(re, im, scratch); + } + finally + { + ArrayPool.Shared.Return(scratch); + } + } + + /// In-place forward DFT using caller-provided scratch of at least doubles. + public void Forward(Span re, Span im, Span scratch) + { + CheckLengths(re, im, scratch); + if (IsPowerOfTwo) Radix2(re, im); + else Bluestein(re, im, scratch); + } + + /// In-place inverse DFT, normalised by 1/N. + public void Inverse(Span re, Span im) + { + // Swapping the real and imaginary parts turns a forward DFT into an unnormalised inverse one. + Forward(im, re); + Scale(re, im, 1.0 / Length); + } + + /// In-place inverse DFT (normalised by 1/N) using caller-provided scratch. + public void Inverse(Span re, Span im, Span scratch) + { + Forward(im, re, scratch); + Scale(re, im, 1.0 / Length); + } + + /// Unnormalised radix-2 transform; the caller guarantees a power-of-two length. + internal void Radix2(Span re, Span im) + { + var n = Length; + var rev = _bitReverse!; + for (var i = 0; i < n; i++) + { + var j = rev[i]; + if (j <= i) continue; + (re[i], re[j]) = (re[j], re[i]); + (im[i], im[j]) = (im[j], im[i]); + } + + ref var r0 = ref MemoryMarshal.GetReference(re); + ref var i0 = ref MemoryMarshal.GetReference(im); + ref var wr0 = ref MemoryMarshal.GetArrayDataReference(_twiddleRe!); + ref var wi0 = ref MemoryMarshal.GetArrayDataReference(_twiddleIm!); + var vc = Vector.Count; + for (var h = 1; h < n; h <<= 1) + { + ref var wr = ref Unsafe.Add(ref wr0, h - 1); + ref var wi = ref Unsafe.Add(ref wi0, h - 1); + for (var start = 0; start < n; start += h << 1) + { + ref var ar = ref Unsafe.Add(ref r0, start); + ref var ai = ref Unsafe.Add(ref i0, start); + ref var br = ref Unsafe.Add(ref r0, start + h); + ref var bi = ref Unsafe.Add(ref i0, start + h); + var k = 0; + if (Vector.IsHardwareAccelerated && h >= vc) + { + // h and vc are powers of two, so h is a multiple of vc: no remainder loop needed. + for (; k < h; k += vc) + { + var u = (nuint)k; + var cr = Vector.LoadUnsafe(ref wr, u); + var ci = Vector.LoadUnsafe(ref wi, u); + var xr = Vector.LoadUnsafe(ref br, u); + var xi = Vector.LoadUnsafe(ref bi, u); + var tr = xr * cr - xi * ci; + var ti = xr * ci + xi * cr; + var yr = Vector.LoadUnsafe(ref ar, u); + var yi = Vector.LoadUnsafe(ref ai, u); + (yr - tr).StoreUnsafe(ref br, u); + (yi - ti).StoreUnsafe(ref bi, u); + (yr + tr).StoreUnsafe(ref ar, u); + (yi + ti).StoreUnsafe(ref ai, u); + } + } + for (; k < h; k++) + { + var cr = Unsafe.Add(ref wr, k); + var ci = Unsafe.Add(ref wi, k); + ref var xr = ref Unsafe.Add(ref br, k); + ref var xi = ref Unsafe.Add(ref bi, k); + ref var yr = ref Unsafe.Add(ref ar, k); + ref var yi = ref Unsafe.Add(ref ai, k); + var tr = xr * cr - xi * ci; + var ti = xr * ci + xi * cr; + xr = yr - tr; + xi = yi - ti; + yr += tr; + yi += ti; + } + } + } + } + + private void Bluestein(Span re, Span im, Span scratch) + { + var n = Length; + var conv = _convolution!; + var m = conv.Length; + var ar = scratch[..m]; + var ai = scratch.Slice(m, m); + var cr = _chirpRe!; + var ci = _chirpIm!; + for (var k = 0; k < n; k++) + { + ar[k] = re[k] * cr[k] - im[k] * ci[k]; + ai[k] = re[k] * ci[k] + im[k] * cr[k]; + } + ar[n..].Clear(); + ai[n..].Clear(); + + conv.Radix2(ar, ai); + var kr = _kernelRe!; + var ki = _kernelIm!; + for (var k = 0; k < m; k++) + { + var xr = ar[k]; + var xi = ai[k]; + ar[k] = xr * kr[k] - xi * ki[k]; + ai[k] = xr * ki[k] + xi * kr[k]; + } + // Unnormalised inverse via the real/imaginary swap; 1/M is folded into the kernel. + conv.Radix2(ai, ar); + + for (var k = 0; k < n; k++) + { + re[k] = ar[k] * cr[k] - ai[k] * ci[k]; + im[k] = ar[k] * ci[k] + ai[k] * cr[k]; + } + } + + private void CheckLengths(Span re, Span im, Span scratch) + { + if (re.Length != Length || im.Length != Length) + throw new ArgumentException($"Expected {Length} samples, got {re.Length} and {im.Length}."); + if (scratch.Length < ScratchLength) + throw new ArgumentException($"Scratch must hold {ScratchLength} doubles.", nameof(scratch)); + } + + private static void Scale(Span re, Span im, double factor) + { + for (var i = 0; i < re.Length; i++) + { + re[i] *= factor; + im[i] *= factor; + } + } + + private static uint ReverseBits(uint v) + { + v = ((v >> 1) & 0x55555555u) | ((v & 0x55555555u) << 1); + v = ((v >> 2) & 0x33333333u) | ((v & 0x33333333u) << 2); + v = ((v >> 4) & 0x0F0F0F0Fu) | ((v & 0x0F0F0F0Fu) << 4); + v = ((v >> 8) & 0x00FF00FFu) | ((v & 0x00FF00FFu) << 8); + return (v >> 16) | (v << 16); + } +} diff --git a/src/Titano.Core/Motion/Fourier.cs b/src/Titano.Core/Motion/Fourier.cs new file mode 100644 index 00000000..f4448008 --- /dev/null +++ b/src/Titano.Core/Motion/Fourier.cs @@ -0,0 +1,269 @@ +using System.Buffers; +using System.Collections.Concurrent; + +namespace Titano.Core.Motion; + +/// +/// Discrete Fourier transforms on split real/imaginary arrays: cached +/// s (twiddles computed once per length), separable 2D transforms (rows then +/// columns, parallel for large images), real-input transforms through the half-length packing trick, +/// and analysis windows. +/// +public static class Fourier +{ + // Below this many samples a 2D transform is cheaper than the Parallel.For overhead. + private const int ParallelThreshold = 1 << 15; + + private static readonly ConcurrentDictionary Plans = new(); + + /// Shared, thread-safe plan for the given length. + public static FftPlan Plan(int length) + { + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(length); + return Plans.GetOrAdd(length, static n => new FftPlan(n)); + } + + /// In-place forward DFT of any length. + public static void Forward(Span re, Span im) => Plan(re.Length).Forward(re, im); + + /// In-place inverse DFT of any length, normalised by 1/N. + public static void Inverse(Span re, Span im) => Plan(re.Length).Inverse(re, im); + + /// In-place forward 2D DFT of a row-major × array. + public static void Forward2D(double[] re, double[] im, int width, int height) => Transform2D(re, im, width, height, inverse: false); + + /// In-place inverse 2D DFT, normalised by 1/(width·height). + public static void Inverse2D(double[] re, double[] im, int width, int height) => Transform2D(re, im, width, height, inverse: true); + + /// + /// Spectrum bins 0..N/2 of a real signal. Even lengths run one complex transform of length N/2 + /// (even samples as real part, odd samples as imaginary part) and split it with the standard + /// post-processing butterflies; odd lengths fall back to a full complex transform. + /// + public static void ForwardReal(ReadOnlySpan signal, Span re, Span im) + { + var n = signal.Length; + ArgumentOutOfRangeException.ThrowIfZero(n, nameof(signal)); + var bins = n / 2 + 1; + if (re.Length < bins || im.Length < bins) throw new ArgumentException($"Spectrum spans must hold {bins} bins."); + if (n == 1) + { + re[0] = signal[0]; + im[0] = 0; + return; + } + if ((n & 1) != 0) + { + var buffer = ArrayPool.Shared.Rent(2 * n); + try + { + var fr = buffer.AsSpan(0, n); + var fi = buffer.AsSpan(n, n); + signal.CopyTo(fr); + fi.Clear(); + Plan(n).Forward(fr, fi); + fr[..bins].CopyTo(re); + fi[..bins].CopyTo(im); + } + finally + { + ArrayPool.Shared.Return(buffer); + } + return; + } + + var m = n / 2; + for (var j = 0; j < m; j++) + { + re[j] = signal[2 * j]; + im[j] = signal[2 * j + 1]; + } + var plan = Plan(m); + plan.Forward(re[..m], im[..m]); + re[m] = re[0]; + im[m] = im[0]; + var wr = plan.HalfTwiddleRe; + var wi = plan.HalfTwiddleIm; + for (var k = 0; k <= m / 2; k++) + { + var k2 = m - k; + double zr = re[k], zi = im[k], yr = re[k2], yi = im[k2]; + // E = (Z_k + conj Z_(m−k)) / 2 is the even-sample spectrum, O = (Z_k − conj Z_(m−k)) / 2i the odd one. + var er = 0.5 * (zr + yr); + var ei = 0.5 * (zi - yi); + var or = 0.5 * (zi + yi); + var oi = -0.5 * (zr - yr); + re[k] = er + wr[k] * or - wi[k] * oi; + im[k] = ei + wr[k] * oi + wi[k] * or; + // X_(m−k) = conj E + W^(m−k) conj O. + re[k2] = er + wr[k2] * or + wi[k2] * oi; + im[k2] = -ei + wi[k2] * or - wr[k2] * oi; + } + } + + /// Inverse of : real signal of length signal.Length from bins 0..N/2. + public static void InverseReal(ReadOnlySpan re, ReadOnlySpan im, Span signal) + { + var n = signal.Length; + ArgumentOutOfRangeException.ThrowIfZero(n, nameof(signal)); + var bins = n / 2 + 1; + if (re.Length < bins || im.Length < bins) throw new ArgumentException($"Spectrum spans must hold {bins} bins."); + if (n == 1) + { + signal[0] = re[0]; + return; + } + + var odd = (n & 1) != 0; + var buffer = ArrayPool.Shared.Rent(odd ? 2 * n : n); + try + { + if (odd) + { + var fr = buffer.AsSpan(0, n); + var fi = buffer.AsSpan(n, n); + for (var k = 0; k < bins; k++) + { + fr[k] = re[k]; + fi[k] = im[k]; + if (k == 0) continue; + fr[n - k] = re[k]; + fi[n - k] = -im[k]; + } + Plan(n).Inverse(fr, fi); + fr.CopyTo(signal); + return; + } + + var m = n / 2; + var zr = buffer.AsSpan(0, m); + var zi = buffer.AsSpan(m, m); + var plan = Plan(m); + var wr = plan.HalfTwiddleRe; + var wi = plan.HalfTwiddleIm; + for (var k = 0; k < m; k++) + { + // E = (X_k + conj X_(m−k)) / 2, O = (X_k − conj X_(m−k)) · conj(W^k) / 2, Z = E + iO. + double xr = re[k], xi = im[k], yr = re[m - k], yi = -im[m - k]; + var er = 0.5 * (xr + yr); + var ei = 0.5 * (xi + yi); + var dr = 0.5 * (xr - yr); + var di = 0.5 * (xi - yi); + var or = dr * wr[k] + di * wi[k]; + var oi = di * wr[k] - dr * wi[k]; + zr[k] = er - oi; + zi[k] = ei + or; + } + plan.Inverse(zr, zi); + for (var j = 0; j < m; j++) + { + signal[2 * j] = zr[j]; + signal[2 * j + 1] = zi[j]; + } + } + finally + { + ArrayPool.Shared.Return(buffer); + } + } + + /// Symmetric Hann window (zero at both ends). + public static void Hann(Span window) => Tukey(window, 1.0); + + /// + /// Symmetric Tukey (tapered cosine) window: flat top with raised-cosine flanks covering a fraction + /// of the length; 0 is rectangular, 1 is Hann. + /// + public static void Tukey(Span window, double alpha) + { + ArgumentOutOfRangeException.ThrowIfLessThan(alpha, 0.0); + ArgumentOutOfRangeException.ThrowIfGreaterThan(alpha, 1.0); + var n = window.Length; + if (n == 1 || alpha == 0) + { + window.Fill(1.0); + return; + } + var half = alpha / 2; + for (var i = 0; i < n; i++) + { + var x = i / (double)(n - 1); + var edge = double.Min(x, 1 - x); + window[i] = edge >= half ? 1.0 : 0.5 * (1 - double.Cos(double.Pi * edge / half)); + } + } + + private static void Transform2D(double[] re, double[] im, int width, int height, bool inverse) + { + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(width); + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(height); + var length = checked(width * height); + if (re.Length < length || im.Length < length) throw new ArgumentException($"Arrays must hold {length} samples."); + + // The inverse is the forward transform with real and imaginary parts swapped, then scaled. + var (a, b) = inverse ? (im, re) : (re, im); + var rows = Plan(width); + var columns = Plan(height); + if (length < ParallelThreshold) + { + var scratch = ArrayPool.Shared.Rent(int.Max(rows.ScratchLength, 2 * height + columns.ScratchLength)); + try + { + for (var y = 0; y < height; y++) rows.Forward(a.AsSpan(y * width, width), b.AsSpan(y * width, width), scratch); + for (var x = 0; x < width; x++) TransformColumn(a, b, width, height, x, columns, scratch); + } + finally + { + ArrayPool.Shared.Return(scratch); + } + } + else + { + Parallel.For( + 0, + height, + () => ArrayPool.Shared.Rent(int.Max(1, rows.ScratchLength)), + (y, _, scratch) => + { + rows.Forward(a.AsSpan(y * width, width), b.AsSpan(y * width, width), scratch); + return scratch; + }, + scratch => ArrayPool.Shared.Return(scratch)); + Parallel.For( + 0, + width, + () => ArrayPool.Shared.Rent(2 * height + columns.ScratchLength), + (x, _, scratch) => + { + TransformColumn(a, b, width, height, x, columns, scratch); + return scratch; + }, + scratch => ArrayPool.Shared.Return(scratch)); + } + + if (!inverse) return; + var scale = 1.0 / length; + for (var i = 0; i < length; i++) + { + re[i] *= scale; + im[i] *= scale; + } + } + + private static void TransformColumn(double[] re, double[] im, int width, int height, int x, FftPlan plan, double[] scratch) + { + var cr = scratch.AsSpan(0, height); + var ci = scratch.AsSpan(height, height); + for (int y = 0, i = x; y < height; y++, i += width) + { + cr[y] = re[i]; + ci[y] = im[i]; + } + plan.Forward(cr, ci, scratch.AsSpan(2 * height)); + for (int y = 0, i = x; y < height; y++, i += width) + { + re[i] = cr[y]; + im[i] = ci[y]; + } + } +} diff --git a/src/Titano.Core/Motion/PhaseCorrelator.cs b/src/Titano.Core/Motion/PhaseCorrelator.cs new file mode 100644 index 00000000..12e9027a --- /dev/null +++ b/src/Titano.Core/Motion/PhaseCorrelator.cs @@ -0,0 +1,273 @@ +namespace Titano.Core.Motion; + +/// +/// Translation between two patches. moving(x, y) ≈ reference(x − Dx, y − Dy): content moved by +/// (+Dx, +Dy) pixels from the reference to the moving patch. is the peak +/// sharpness ratio 1 − second/primary peak, in [0, 1]; is the correlation peak height +/// (1 for a perfect translation, near 0 for unrelated content). +/// +public readonly record struct PhaseCorrelation(double Dx, double Dy, double Confidence, double Peak); + +/// Tuning for . +public sealed record PhaseCorrelatorOptions +{ + /// Tukey taper fraction of the analysis window (1 = Hann). Suppresses the patch-border discontinuity. + public double WindowTaper { get; init; } = 0.5; + + /// + /// Spatial σ (px) of the Gaussian low-pass applied to the normalised cross-power spectrum. It shapes the + /// correlation peak into a sampled Gaussian, which makes the log-parabola sub-pixel fit unbiased, and + /// attenuates the noisiest high frequencies. + /// + public double PeakSigma { get; init; } = 1.0; + + /// Largest |shift| searched per axis (px); 0 searches the whole surface. + public int MaxShift { get; init; } + + /// Default options. + public static PhaseCorrelatorOptions Default { get; } = new(); +} + +/// +/// Kuglin & Hines (1975) phase correlation between two equally sized luminance patches: weighted-mean +/// removal, Tukey window, one complex FFT carrying both real patches (reference as real part, moving as +/// imaginary part), normalised cross-power spectrum with a relative epsilon and a Gaussian low-pass, +/// inverse FFT, integer peak search and a separable Gaussian (log-parabola) sub-pixel fit. +/// Holds its working buffers, so one instance must not be used by two threads at once. +/// +public sealed class PhaseCorrelator +{ + // Relative regulariser of |cross power|: keeps empty bins from being amplified to unit magnitude. + private const double SpectrumEpsilon = 1e-6; + + private readonly double[] _window; + private readonly double[] _lowPass; + private readonly double[] _re; + private readonly double[] _im; + private readonly int _maxShiftX; + private readonly int _maxShiftY; + private readonly int _exclusion; + private readonly double _windowSum; + + public PhaseCorrelator(int width, int height, PhaseCorrelatorOptions? options = null) + { + ArgumentOutOfRangeException.ThrowIfLessThan(width, 4); + ArgumentOutOfRangeException.ThrowIfLessThan(height, 4); + Options = options ?? PhaseCorrelatorOptions.Default; + Width = width; + Height = height; + var n = width * height; + _re = new double[n]; + _im = new double[n]; + + var wxArray = new double[width]; + var wyArray = new double[height]; + Fourier.Tukey(wxArray, Options.WindowTaper); + Fourier.Tukey(wyArray, Options.WindowTaper); + _window = new double[n]; + for (var y = 0; y < height; y++) + { + for (var x = 0; x < width; x++) + { + var w = wyArray[y] * wxArray[x]; + _window[y * width + x] = w; + _windowSum += w; + } + } + + // Gaussian of spatial σ in the frequency domain: exp(−2π²σ²|f|²), f in cycles per pixel. + _lowPass = new double[n]; + var sigma = Options.PeakSigma; + var k = -2.0 * double.Pi * double.Pi * sigma * sigma; + for (var v = 0; v < height; v++) + { + var fy = Signed(v, height) / (double)height; + for (var u = 0; u < width; u++) + { + var fx = Signed(u, width) / (double)width; + _lowPass[v * width + u] = sigma > 0 ? double.Exp(k * (fx * fx + fy * fy)) : 1.0; + } + } + _lowPass[0] = 0; // DC carries no shift information. + double sum = 0; + foreach (var g in _lowPass) sum += g; + // Normalise so that a perfect translation produces a unit peak. + var norm = n / sum; + for (var i = 0; i < n; i++) _lowPass[i] *= norm; + + var limitX = width / 2 - 1; + var limitY = height / 2 - 1; + _maxShiftX = Options.MaxShift > 0 ? int.Min(Options.MaxShift, limitX) : limitX; + _maxShiftY = Options.MaxShift > 0 ? int.Min(Options.MaxShift, limitY) : limitY; + _exclusion = int.Max(2, (int)double.Ceiling(3 * double.Max(sigma, 0.5))); + } + + public int Width { get; } + + public int Height { get; } + + public PhaseCorrelatorOptions Options { get; } + + /// Correlates two row-major patches of × samples. + public PhaseCorrelation Correlate(ReadOnlySpan reference, ReadOnlySpan moving) => + Correlate(reference, moving, Width, 0, 0); + + /// + /// Correlates the patches at (, ) of two equally sized planes + /// with row stride . + /// + public PhaseCorrelation Correlate(ReadOnlySpan referencePlane, ReadOnlySpan movingPlane, int planeWidth, int x0, int y0) + { + var w = Width; + var h = Height; + if (x0 < 0 || y0 < 0 || x0 + w > planeWidth) throw new ArgumentOutOfRangeException(nameof(x0), "Patch outside the plane."); + var last = (y0 + h - 1) * planeWidth + x0 + w; + if (last > referencePlane.Length || last > movingPlane.Length) throw new ArgumentOutOfRangeException(nameof(y0), "Patch outside the plane."); + + // Window-weighted means, so the windowed patches have exactly zero DC. + double ma = 0, mb = 0; + for (var y = 0; y < h; y++) + { + var ra = referencePlane.Slice((y0 + y) * planeWidth + x0, w); + var rb = movingPlane.Slice((y0 + y) * planeWidth + x0, w); + var win = _window.AsSpan(y * w, w); + for (var x = 0; x < w; x++) + { + ma += win[x] * ra[x]; + mb += win[x] * rb[x]; + } + } + ma /= _windowSum; + mb /= _windowSum; + for (var y = 0; y < h; y++) + { + var ra = referencePlane.Slice((y0 + y) * planeWidth + x0, w); + var rb = movingPlane.Slice((y0 + y) * planeWidth + x0, w); + var o = y * w; + for (var x = 0; x < w; x++) + { + _re[o + x] = (ra[x] - ma) * _window[o + x]; + _im[o + x] = (rb[x] - mb) * _window[o + x]; + } + } + + Fourier.Forward2D(_re, _im, w, h); + CrossPowerSpectrum(); + Fourier.Inverse2D(_re, _im, w, h); + return LocatePeak(); + } + + /// + /// Replaces the packed spectrum Z = A + iB with the normalised, low-passed cross power conj(A)·B / |conj(A)·B|. + /// A and B are recovered from Hermitian symmetry: A_k = (Z_k + conj Z_−k)/2, B_k = (Z_k − conj Z_−k)/2i. + /// + private void CrossPowerSpectrum() + { + var w = Width; + var h = Height; + var re = _re; + var im = _im; + double magnitudeSum = 0; + for (var v = 0; v < h; v++) + { + var mv = v == 0 ? 0 : h - v; + for (var u = 0; u < w; u++) + { + var i = v * w + u; + var j = mv * w + (u == 0 ? 0 : w - u); + if (j < i) continue; + double zr = re[i], zi = im[i], yr = re[j], yi = im[j]; + var ar = 0.5 * (zr + yr); + var ai = 0.5 * (zi - yi); + var br = 0.5 * (zi + yi); + var bi = -0.5 * (zr - yr); + // conj(A)·B; the mirrored bin holds its conjugate because the correlation is real. + var cr = ar * br + ai * bi; + var ci = ar * bi - ai * br; + re[i] = cr; + im[i] = ci; + re[j] = cr; + im[j] = -ci; + var mag = double.Sqrt(cr * cr + ci * ci); + magnitudeSum += i == j ? mag : 2 * mag; + } + } + + var epsilon = SpectrumEpsilon * magnitudeSum / (w * h) + double.Epsilon; + var lowPass = _lowPass; + for (var i = 0; i < re.Length; i++) + { + var mag = double.Sqrt(re[i] * re[i] + im[i] * im[i]); + var f = lowPass[i] / (mag + epsilon); + re[i] *= f; + im[i] *= f; + } + } + + private PhaseCorrelation LocatePeak() + { + var w = Width; + var h = Height; + var c = _re; + int px = 0, py = 0; + var best = double.NegativeInfinity; + for (var dy = -_maxShiftY; dy <= _maxShiftY; dy++) + { + var row = Wrap(dy, h) * w; + for (var dx = -_maxShiftX; dx <= _maxShiftX; dx++) + { + var value = c[row + Wrap(dx, w)]; + if (value > best) + { + best = value; + px = dx; + py = dy; + } + } + } + + double At(int dx, int dy) => c[Wrap(dy, h) * w + Wrap(dx, w)]; + var fx = SubPixel(At(px - 1, py), best, At(px + 1, py)); + var fy = SubPixel(At(px, py - 1), best, At(px, py + 1)); + + // Second peak: the largest value outside the main lobe (wrap-aware Chebyshev distance). + var second = double.NegativeInfinity; + for (var dy = -_maxShiftY; dy <= _maxShiftY; dy++) + { + var row = Wrap(dy, h) * w; + var nearY = int.Abs(dy - py) <= _exclusion; + for (var dx = -_maxShiftX; dx <= _maxShiftX; dx++) + { + if (nearY && int.Abs(dx - px) <= _exclusion) continue; + var value = c[row + Wrap(dx, w)]; + if (value > second) second = value; + } + } + + var confidence = best > 0 ? double.Clamp(1 - double.Max(second, 0) / best, 0, 1) : 0; + return new PhaseCorrelation(px + fx, py + fy, confidence, double.Max(best, 0)); + } + + /// Vertex of the parabola through log values (exact for a sampled Gaussian), or of the plain + /// parabola when a neighbour is not positive; clamped to ±0.5 px. + private static double SubPixel(double left, double centre, double right) + { + double offset; + if (left > 0 && centre > 0 && right > 0) + { + double l = double.Log(left), c = double.Log(centre), r = double.Log(right); + var d = l - 2 * c + r; + offset = d < 0 ? 0.5 * (l - r) / d : 0; + } + else + { + var d = left - 2 * centre + right; + offset = d < 0 ? 0.5 * (left - right) / d : 0; + } + return double.Clamp(offset, -0.5, 0.5); + } + + private static int Signed(int index, int length) => index <= length / 2 ? index : index - length; + + private static int Wrap(int index, int length) => index < 0 ? index + length : index; +} diff --git a/tests/Titano.Core.Tests/Motion/FourierTests.cs b/tests/Titano.Core.Tests/Motion/FourierTests.cs new file mode 100644 index 00000000..621d1fd6 --- /dev/null +++ b/tests/Titano.Core.Tests/Motion/FourierTests.cs @@ -0,0 +1,176 @@ +using Titano.Core.Motion; +using Titano.Testing; + +namespace Titano.Core.Tests.Motion; + +public sealed class FourierTests +{ + [Theory] + [Case(1)] + [Case(2)] + [Case(64)] + [Case(1024)] + [Case(4096)] + public void PowerOfTwoRoundTrip(int n) + { + var (re, im) = RandomSignal(n, seed: n); + var (r0, i0) = ((double[])re.Clone(), (double[])im.Clone()); + Fourier.Forward(re, im); + Fourier.Inverse(re, im); + Assert.LessOrEqual(MaxError(r0, i0, re, im), 1e-5); + } + + [Theory] + [Case(3)] + [Case(7)] + [Case(13)] + [Case(97)] + [Case(251)] + [Case(1009)] + public void BluesteinMatchesDirectDftForPrimes(int n) + { + var (re, im) = RandomSignal(n, seed: n); + var (er, ei) = Dft(re, im); + Fourier.Forward(re, im); + var scale = double.Sqrt(n); + Assert.LessOrEqual(MaxError(er, ei, re, im) / scale, 1e-9); + } + + [Theory] + [Case(12)] + [Case(100)] + [Case(1000)] + public void CompositeLengthsRoundTrip(int n) + { + var (re, im) = RandomSignal(n, seed: 3 * n); + var (r0, i0) = ((double[])re.Clone(), (double[])im.Clone()); + Fourier.Forward(re, im); + Fourier.Inverse(re, im); + Assert.LessOrEqual(MaxError(r0, i0, re, im), 1e-5); + } + + [Theory] + [Case(64)] + [Case(100)] + [Case(15)] + public void RealTransformMatchesComplexAndRoundTrips(int n) + { + var (signal, _) = RandomSignal(n, seed: 11 * n); + var bins = n / 2 + 1; + var hr = new double[bins]; + var hi = new double[bins]; + Fourier.ForwardReal(signal, hr, hi); + + var re = (double[])signal.Clone(); + var im = new double[n]; + Fourier.Forward(re, im); + Assert.LessOrEqual(MaxError(re.AsSpan(0, bins).ToArray(), im.AsSpan(0, bins).ToArray(), hr, hi), 1e-9); + + var back = new double[n]; + Fourier.InverseReal(hr, hi, back); + for (var i = 0; i < n; i++) Assert.Approximately(signal[i], back[i], 1e-9); + } + + [Theory] + [Case(64, 64)] + [Case(48, 36)] + [Case(256, 160)] + public void TwoDimensionalRoundTripAndShiftTheorem(int width, int height) + { + var (re, im) = RandomSignal(width * height, seed: width + height); + var (r0, i0) = ((double[])re.Clone(), (double[])im.Clone()); + Fourier.Forward2D(re, im, width, height); + // A unit impulse at (1, 0) transforms to e^(−2πiu/W) on every row: check one bin of a separate impulse. + var dr = new double[width * height]; + var di = new double[width * height]; + dr[1] = 1; + Fourier.Forward2D(dr, di, width, height); + var (s, c) = double.SinCos(-2 * double.Pi * 3 / width); + Assert.Approximately(c, dr[2 * width + 3], 1e-12); + Assert.Approximately(s, di[2 * width + 3], 1e-12); + + Fourier.Inverse2D(re, im, width, height); + Assert.LessOrEqual(MaxError(r0, i0, re, im), 1e-5); + } + + [Test] + public void ParsevalHolds() + { + const int n = 512; + var (re, im) = RandomSignal(n, seed: 5); + double time = 0, freq = 0; + for (var i = 0; i < n; i++) time += re[i] * re[i] + im[i] * im[i]; + Fourier.Forward(re, im); + for (var i = 0; i < n; i++) freq += re[i] * re[i] + im[i] * im[i]; + Assert.Approximately(time, freq / n, 1e-9 * time); + } + + [Test] + public void WindowsHaveExpectedShape() + { + var hann = new double[9]; + Fourier.Hann(hann); + Assert.Approximately(0, hann[0], 1e-15); + Assert.Approximately(1, hann[4], 1e-15); + Assert.Approximately(0.5, hann[2], 1e-12); + Assert.Approximately(hann[1], hann[7], 1e-15); + + var rect = new double[9]; + Fourier.Tukey(rect, 0); + Assert.True(rect.All(v => v == 1)); + + var tukey = new double[101]; + Fourier.Tukey(tukey, 0.5); + Assert.Approximately(0, tukey[0], 1e-15); + Assert.Equal(1.0, tukey[25]); + Assert.Equal(1.0, tukey[75]); + // Mid-taper (x = 0.125) is at half height; the flanks rise monotonically and mirror each other. + Assert.InRange(tukey[12], 0.45, 0.5); + Assert.InRange(tukey[13], 0.5, 0.55); + Assert.Monotonic(tukey.Take(26).ToArray()); + for (var i = 0; i < 101; i++) Assert.Approximately(tukey[i], tukey[100 - i], 1e-15); + } + + [Test] + public void PlansAreCached() => Assert.Same(Fourier.Plan(96), Fourier.Plan(96)); + + private static (double[] Re, double[] Im) RandomSignal(int n, int seed) + { + var random = new Random(seed); + var re = new double[n]; + var im = new double[n]; + for (var i = 0; i < n; i++) + { + re[i] = random.NextDouble() * 2 - 1; + im[i] = random.NextDouble() * 2 - 1; + } + return (re, im); + } + + private static (double[] Re, double[] Im) Dft(double[] re, double[] im) + { + var n = re.Length; + var or = new double[n]; + var oi = new double[n]; + for (var k = 0; k < n; k++) + { + double sr = 0, si = 0; + for (var t = 0; t < n; t++) + { + var (s, c) = double.SinCos(-2 * double.Pi * ((long)k * t % n) / n); + sr += re[t] * c - im[t] * s; + si += re[t] * s + im[t] * c; + } + or[k] = sr; + oi[k] = si; + } + return (or, oi); + } + + private static double MaxError(double[] ar, double[] ai, double[] br, double[] bi) + { + double max = 0; + for (var i = 0; i < ar.Length; i++) max = double.Max(max, double.Max(double.Abs(ar[i] - br[i]), double.Abs(ai[i] - bi[i]))); + return max; + } +} diff --git a/tests/Titano.Core.Tests/Motion/PhaseCorrelatorTests.cs b/tests/Titano.Core.Tests/Motion/PhaseCorrelatorTests.cs new file mode 100644 index 00000000..c268c527 --- /dev/null +++ b/tests/Titano.Core.Tests/Motion/PhaseCorrelatorTests.cs @@ -0,0 +1,83 @@ +using Titano.Core.Motion; +using Titano.Testing; + +namespace Titano.Core.Tests.Motion; + +public sealed class PhaseCorrelatorTests +{ + private const int Size = 256; + private const int Patch = 128; + + private readonly TestContext _context; + + public PhaseCorrelatorTests(TestContext context) => _context = context; + + [Theory] + [Case("noise", 3.25, -1.6)] + [Case("noise", 0.5, 0.5)] + [Case("noise", -7.8, 2.1)] + [Case("noise", 0.0, 0.0)] + [Case("natural", 3.25, -1.6)] + [Case("natural", 0.5, 0.5)] + [Case("natural", -7.8, 2.1)] + [Case("natural", 12.3, -9.45)] + public void SubPixelErrorWithinTenthOfPixel(string texture, double dx, double dy) + { + var image = texture == "noise" ? SyntheticImages.Noise(Size, 0.8, seed: 7) : SyntheticImages.Natural(Size, seed: 7); + var shifted = SyntheticImages.Shift(image, Size, dx, dy); + var reference = SyntheticImages.Crop(image, Size, 64, 64, Patch, Patch); + var moving = SyntheticImages.Crop(shifted, Size, 64, 64, Patch, Patch); + + var result = new PhaseCorrelator(Patch, Patch).Correlate(reference, moving); + var error = double.Hypot(result.Dx - dx, result.Dy - dy); + _context.Log($"{texture} ({dx}, {dy}) → ({result.Dx:F4}, {result.Dy:F4}) error {error:F4} px, confidence {result.Confidence:F3}, peak {result.Peak:F3}"); + Assert.LessOrEqual(error, 0.10); + Assert.Greater(result.Confidence, 0.5); + } + + [Test] + public void NonPowerOfTwoPatchesUseBluestein() + { + var image = SyntheticImages.Natural(Size, seed: 3); + var shifted = SyntheticImages.Shift(image, Size, -2.4, 1.3); + var reference = SyntheticImages.Crop(image, Size, 60, 70, 96, 80); + var moving = SyntheticImages.Crop(shifted, Size, 60, 70, 96, 80); + var result = new PhaseCorrelator(96, 80).Correlate(reference, moving); + Assert.Within(-2.4, 1.3, result.Dx, result.Dy, 0.10); + } + + [Test] + public void PatchesAreReadFromLargerPlanes() + { + var image = SyntheticImages.Noise(Size, 1.0, seed: 9); + var shifted = SyntheticImages.Shift(image, Size, 1.75, -0.25); + var a = SyntheticImages.Crop(image, Size, 0, 0, Size, Size); + var b = SyntheticImages.Crop(shifted, Size, 0, 0, Size, Size); + var result = new PhaseCorrelator(64, 64).Correlate(a, b, Size, 100, 40); + Assert.Within(1.75, -0.25, result.Dx, result.Dy, 0.10); + } + + [Test] + public void UnrelatedContentHasLowConfidence() + { + var a = SyntheticImages.Crop(SyntheticImages.Noise(Size, 0.8, seed: 1), Size, 0, 0, Patch, Patch); + var b = SyntheticImages.Crop(SyntheticImages.Noise(Size, 0.8, seed: 2), Size, 0, 0, Patch, Patch); + var result = new PhaseCorrelator(Patch, Patch).Correlate(a, b); + _context.Log($"confidence {result.Confidence:F3}, peak {result.Peak:F3}"); + Assert.Less(result.Confidence, 0.4); + Assert.Less(result.Peak, 0.2); + } + + [Test] + public void MaxShiftLimitsTheSearch() + { + var image = SyntheticImages.Noise(Size, 0.8, seed: 4); + var shifted = SyntheticImages.Shift(image, Size, 20, 0); + var a = SyntheticImages.Crop(image, Size, 64, 64, Patch, Patch); + var b = SyntheticImages.Crop(shifted, Size, 64, 64, Patch, Patch); + var free = new PhaseCorrelator(Patch, Patch).Correlate(a, b); + Assert.Within(20, 0, free.Dx, free.Dy, 0.10); + var limited = new PhaseCorrelator(Patch, Patch, new PhaseCorrelatorOptions { MaxShift = 8 }).Correlate(a, b); + Assert.LessOrEqual(double.Abs(limited.Dx), 8.5); + } +} diff --git a/tests/Titano.Core.Tests/Motion/SyntheticImages.cs b/tests/Titano.Core.Tests/Motion/SyntheticImages.cs new file mode 100644 index 00000000..921c8d9d --- /dev/null +++ b/tests/Titano.Core.Tests/Motion/SyntheticImages.cs @@ -0,0 +1,122 @@ +using Titano.Core.Imaging; +using Titano.Core.Motion; + +namespace Titano.Core.Tests.Motion; + +/// Deterministic synthetic textures (periodic, band-limited) and exact sub-pixel shifts by Fourier phase ramp. +internal static class SyntheticImages +{ + /// Gaussian-filtered white noise: spectral envelope exp(−2π²σ²|f|²), mean 0.5, standard deviation 0.15. + public static double[] Noise(int size, double sigma, int seed) => + Synthesize(size, seed, f => double.Exp(-2 * double.Pi * double.Pi * sigma * sigma * f * f)); + + /// Natural-image-like texture: amplitude ∝ 1/f^1.4 with a soft high-frequency roll-off. + public static double[] Natural(int size, int seed) => + Synthesize(size, seed, f => f == 0 ? 0 : double.Pow(f * size, -1.4) * double.Exp(-f * f / (2 * 0.2 * 0.2))); + + /// Periodic shift with sub-pixel accuracy: result(x, y) = image(x − dx, y − dy). + public static double[] Shift(double[] image, int size, double dx, double dy) + { + var re = (double[])image.Clone(); + var im = new double[re.Length]; + Fourier.Forward2D(re, im, size, size); + for (var v = 0; v < size; v++) + { + for (var u = 0; u < size; u++) + { + var (s, c) = double.SinCos(-2 * double.Pi * (Signed(u, size) * dx + Signed(v, size) * dy) / size); + var i = v * size + u; + (re[i], im[i]) = (re[i] * c - im[i] * s, re[i] * s + im[i] * c); + } + } + Fourier.Inverse2D(re, im, size, size); + return re; + } + + /// Copies a rectangle of a square periodic image into a float plane. + public static float[] Crop(double[] image, int size, int x0, int y0, int width, int height) + { + var plane = new float[width * height]; + for (var y = 0; y < height; y++) + { + for (var x = 0; x < width; x++) plane[y * width + x] = (float)image[((y0 + y) % size) * size + (x0 + x) % size]; + } + return plane; + } + + /// Single-channel buffer from a plane. + public static ImageBuffer ToBuffer(float[] plane, int width, int height) + { + var buffer = new ImageBuffer(width, height, 1, clear: false); + plane.AsSpan().CopyTo(buffer.Plane(0)); + return buffer; + } + + /// RGB buffer whose channels are scaled copies of a plane (distinct but correlated colours). + public static ImageBuffer ToRgb(float[] plane, int width, int height) + { + var buffer = new ImageBuffer(width, height, 3, clear: false); + for (var c = 0; c < 3; c++) + { + var dst = buffer.Plane(c); + var gain = 0.6f + 0.2f * c; + for (var i = 0; i < plane.Length; i++) dst[i] = plane[i] * gain; + } + return buffer; + } + + /// Peak signal-to-noise ratio in dB for unit peak over the window [x0, x1)×[y0, y1). + public static double Psnr(ReadOnlySpan a, ReadOnlySpan b, int width, int x0, int y0, int x1, int y1) + { + double sum = 0; + var n = 0; + for (var y = y0; y < y1; y++) + { + for (var x = x0; x < x1; x++) + { + var d = a[y * width + x] - (double)b[y * width + x]; + sum += d * d; + n++; + } + } + var mse = sum / n; + return mse == 0 ? double.PositiveInfinity : 10 * double.Log10(1 / mse); + } + + private static double[] Synthesize(int size, int seed, Func envelope) + { + var random = new Random(seed); + var re = new double[size * size]; + var im = new double[size * size]; + for (var v = 0; v < size; v++) + { + for (var u = 0; u < size; u++) + { + // Nyquist bins stay empty so the phase-ramp shift is exact for a real image. + if (u == size / 2 || v == size / 2) continue; + double fx = Signed(u, size) / (double)size, fy = Signed(v, size) / (double)size; + var a = envelope(double.Sqrt(fx * fx + fy * fy)); + re[v * size + u] = a * Gaussian(random); + im[v * size + u] = a * Gaussian(random); + } + } + re[0] = im[0] = 0; + Fourier.Inverse2D(re, im, size, size); + double mean = 0, sq = 0; + foreach (var x in re) mean += x; + mean /= re.Length; + foreach (var x in re) sq += (x - mean) * (x - mean); + var scale = 0.15 / double.Sqrt(sq / re.Length); + for (var i = 0; i < re.Length; i++) re[i] = 0.5 + (re[i] - mean) * scale; + return re; + } + + private static double Gaussian(Random random) + { + var u1 = 1.0 - random.NextDouble(); + var u2 = random.NextDouble(); + return double.Sqrt(-2 * double.Log(u1)) * double.Cos(2 * double.Pi * u2); + } + + private static int Signed(int index, int length) => index <= length / 2 ? index : index - length; +}