feat(motion): add FFT plans and Kuglin–Hines phase correlation

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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_012iLMBNwBE2GnphAsu4cjaC
This commit is contained in:
Claude committed 2026-10-02 09:19:18 +00:00
1 parent 800843d2ae
commit bda76ccff2
6 files changed
+1199

No files matched your search

+276
View File
@@ -0,0 +1,276 @@
using System.Buffers;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
namespace Titano.Core.Motion;
/// <summary>
/// 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); <see cref="Inverse(Span{double}, Span{double})"/> is normalised by 1/N.
/// Immutable and thread-safe; obtain shared instances from <see cref="Fourier.Plan"/>.
/// </summary>
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;
}
}
/// <summary>Transform length N.</summary>
public int Length { get; }
/// <summary>True when the radix-2 path is used.</summary>
public bool IsPowerOfTwo { get; }
/// <summary>Doubles of scratch the scratch-taking overloads need (0 for powers of two).</summary>
public int ScratchLength { get; }
internal double[] HalfTwiddleRe { get; }
internal double[] HalfTwiddleIm { get; }
/// <summary>In-place forward DFT. Bluestein lengths rent their scratch from the shared pool.</summary>
public void Forward(Span<double> re, Span<double> im)
{
if (ScratchLength == 0)
{
Forward(re, im, Span<double>.Empty);
return;
}
var scratch = ArrayPool<double>.Shared.Rent(ScratchLength);
try
{
Forward(re, im, scratch);
}
finally
{
ArrayPool<double>.Shared.Return(scratch);
}
}
/// <summary>In-place forward DFT using caller-provided scratch of at least <see cref="ScratchLength"/> doubles.</summary>
public void Forward(Span<double> re, Span<double> im, Span<double> scratch)
{
CheckLengths(re, im, scratch);
if (IsPowerOfTwo) Radix2(re, im);
else Bluestein(re, im, scratch);
}
/// <summary>In-place inverse DFT, normalised by 1/N.</summary>
public void Inverse(Span<double> re, Span<double> 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);
}
/// <summary>In-place inverse DFT (normalised by 1/N) using caller-provided scratch.</summary>
public void Inverse(Span<double> re, Span<double> im, Span<double> scratch)
{
Forward(im, re, scratch);
Scale(re, im, 1.0 / Length);
}
/// <summary>Unnormalised radix-2 transform; the caller guarantees a power-of-two length.</summary>
internal void Radix2(Span<double> re, Span<double> 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<double>.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<double> re, Span<double> im, Span<double> 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<double> re, Span<double> im, Span<double> 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<double> re, Span<double> 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);
}
}
+269
View File
@@ -0,0 +1,269 @@
using System.Buffers;
using System.Collections.Concurrent;
namespace Titano.Core.Motion;
/// <summary>
/// Discrete Fourier transforms on split real/imaginary <see cref="double"/> arrays: cached
/// <see cref="FftPlan"/>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.
/// </summary>
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<int, FftPlan> Plans = new();
/// <summary>Shared, thread-safe plan for the given length.</summary>
public static FftPlan Plan(int length)
{
ArgumentOutOfRangeException.ThrowIfNegativeOrZero(length);
return Plans.GetOrAdd(length, static n => new FftPlan(n));
}
/// <summary>In-place forward DFT of any length.</summary>
public static void Forward(Span<double> re, Span<double> im) => Plan(re.Length).Forward(re, im);
/// <summary>In-place inverse DFT of any length, normalised by 1/N.</summary>
public static void Inverse(Span<double> re, Span<double> im) => Plan(re.Length).Inverse(re, im);
/// <summary>In-place forward 2D DFT of a row-major <paramref name="width"/>×<paramref name="height"/> array.</summary>
public static void Forward2D(double[] re, double[] im, int width, int height) => Transform2D(re, im, width, height, inverse: false);
/// <summary>In-place inverse 2D DFT, normalised by 1/(width·height).</summary>
public static void Inverse2D(double[] re, double[] im, int width, int height) => Transform2D(re, im, width, height, inverse: true);
/// <summary>
/// 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.
/// </summary>
public static void ForwardReal(ReadOnlySpan<double> signal, Span<double> re, Span<double> 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<double>.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<double>.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;
}
}
/// <summary>Inverse of <see cref="ForwardReal"/>: real signal of length <c>signal.Length</c> from bins 0..N/2.</summary>
public static void InverseReal(ReadOnlySpan<double> re, ReadOnlySpan<double> im, Span<double> 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<double>.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<double>.Shared.Return(buffer);
}
}
/// <summary>Symmetric Hann window (zero at both ends).</summary>
public static void Hann(Span<double> window) => Tukey(window, 1.0);
/// <summary>
/// Symmetric Tukey (tapered cosine) window: flat top with raised-cosine flanks covering a fraction
/// <paramref name="alpha"/> of the length; 0 is rectangular, 1 is Hann.
/// </summary>
public static void Tukey(Span<double> 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<double>.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<double>.Shared.Return(scratch);
}
}
else
{
Parallel.For(
0,
height,
() => ArrayPool<double>.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<double>.Shared.Return(scratch));
Parallel.For(
0,
width,
() => ArrayPool<double>.Shared.Rent(2 * height + columns.ScratchLength),
(x, _, scratch) =>
{
TransformColumn(a, b, width, height, x, columns, scratch);
return scratch;
},
scratch => ArrayPool<double>.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];
}
}
}
+273
View File
@@ -0,0 +1,273 @@
namespace Titano.Core.Motion;
/// <summary>
/// Translation between two patches. <c>moving(x, y) ≈ reference(x − Dx, y − Dy)</c>: content moved by
/// (+Dx, +Dy) pixels from the reference to the moving patch. <see cref="Confidence"/> is the peak
/// sharpness ratio 1 − second/primary peak, in [0, 1]; <see cref="Peak"/> is the correlation peak height
/// (1 for a perfect translation, near 0 for unrelated content).
/// </summary>
public readonly record struct PhaseCorrelation(double Dx, double Dy, double Confidence, double Peak);
/// <summary>Tuning for <see cref="PhaseCorrelator"/>.</summary>
public sealed record PhaseCorrelatorOptions
{
/// <summary>Tukey taper fraction of the analysis window (1 = Hann). Suppresses the patch-border discontinuity.</summary>
public double WindowTaper { get; init; } = 0.5;
/// <summary>
/// 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.
/// </summary>
public double PeakSigma { get; init; } = 1.0;
/// <summary>Largest |shift| searched per axis (px); 0 searches the whole surface.</summary>
public int MaxShift { get; init; }
/// <summary>Default options.</summary>
public static PhaseCorrelatorOptions Default { get; } = new();
}
/// <summary>
/// Kuglin &amp; 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.
/// </summary>
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; }
/// <summary>Correlates two row-major patches of <see cref="Width"/>×<see cref="Height"/> samples.</summary>
public PhaseCorrelation Correlate(ReadOnlySpan<float> reference, ReadOnlySpan<float> moving) =>
Correlate(reference, moving, Width, 0, 0);
/// <summary>
/// Correlates the patches at (<paramref name="x0"/>, <paramref name="y0"/>) of two equally sized planes
/// with row stride <paramref name="planeWidth"/>.
/// </summary>
public PhaseCorrelation Correlate(ReadOnlySpan<float> referencePlane, ReadOnlySpan<float> 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();
}
/// <summary>
/// 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.
/// </summary>
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));
}
/// <summary>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.</summary>
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;
}