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:
6 files changed
+1199
No files matched your search
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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 & 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;
|
||||
}
|
||||
Reference in new issue
Block a user