using System.Buffers;
using System.Numerics;
using Titano.Core.Imaging;
using Titano.Core.Imaging.Geometry;
namespace Titano.Core.Motion;
/// Tuning for .
public sealed record StabilizerOptions
{
/// Phase-correlation patch side (power of two recommended); 0 picks min(W, H)/3 rounded down to a power of two, 8..256.
public int PatchSize { get; init; }
///
/// Texture gate: minimum gradient energy of a patch (perceptual luminance² per px², measured after a
/// 3×3 binomial blur, with the expected contribution of the estimated noise removed).
///
public double TextureThreshold { get; init; } = 1e-5;
/// Minimum phase-correlation peak sharpness for a patch vector to be used.
public double MinimumConfidence { get; init; } = 0.3;
/// Residual (px) under which a patch vector is an inlier of a similarity hypothesis.
public double InlierThreshold { get; init; } = 1.0;
/// Largest plausible frame-to-frame rotation (radians) of a hypothesis.
public double MaxRotation { get; init; } = 0.05;
/// Largest plausible frame-to-frame |ln scale| of a hypothesis.
public double MaxScaleChange { get; init; } = 0.05;
/// σ (frames) of the Gaussian-weighted local linear regression that smooths the camera path; 0 disables.
public double SmoothingSigma { get; init; } = 12;
/// Shake (RMS px) above which stabilisation is recommended (brief §10.3).
public double RecommendThreshold { get; init; } = 0.3;
}
/// Similarity between consecutive frames: maps previous-frame pixel coordinates to current-frame ones.
public readonly record struct FrameMotion(Affine2D Transform, int ValidPatches, int Inliers)
{
/// False when no patch passed the texture and confidence gates (identity assumed).
public bool Estimated => ValidPatches > 0;
}
/// Outcome of ; every transform uses pixel-centre coordinates.
public sealed record StabilizationResult(
IReadOnlyList Motions,
IReadOnlyList CameraPath,
IReadOnlyList SmoothedPath,
IReadOnlyList Corrections,
double ShakeRms,
double CropScale,
bool Recommended)
{
/// Fraction of the frame area lost to the centred crop that hides every correction's borders, 1 − scale².
public double CropCost => 1 - CropScale * CropScale;
}
///
/// Video stabilisation. Per frame: Kuglin–Hines phase correlation of nine patches on a 3×3 grid against the
/// previous frame (patches analysed in parallel), texture gating (noise-compensated gradient energy,
/// Immerkær 1996 noise estimate; a textureless frame yields no vectors and therefore no motion), then a
/// robust similarity fit (exhaustive two-point hypotheses as deterministic RANSAC, plausibility limits,
/// weighted least squares on the inliers). accumulates the camera path, smooths its
/// translation, rotation and log-scale with Gaussian-weighted local linear regression (pans survive
/// unchanged), and reports per-frame corrections (smoothed ∘ raw⁻¹), shake RMS and the crop needed to hide
/// borders. Memory is two luminance planes plus one small record per frame.
///
public sealed class Stabilizer : IDisposable
{
// Expected smoothed-gradient energy of unit-variance white noise: 2 · Σ([−1 −2 0 2 1]/8)² · Σ([1 2 1]/4)².
private const double NoiseGradientGain = 2 * (10.0 / 64) * (6.0 / 16);
private readonly PhaseCorrelator[] _correlators;
private readonly PhaseCorrelation?[] _results = new PhaseCorrelation?[9];
private readonly (int X, int Y)[] _patches;
private readonly double[] _previousEnergy;
private readonly double[] _currentEnergy;
private readonly float[][] _smooth;
private readonly Match[] _matches = new Match[9];
private readonly List _motions = [];
private float[]? _previous;
private float[]? _current;
public Stabilizer(int width, int height, StabilizerOptions? options = null)
{
ArgumentOutOfRangeException.ThrowIfLessThan(width, 16);
ArgumentOutOfRangeException.ThrowIfLessThan(height, 16);
Options = options ?? new StabilizerOptions();
Width = width;
Height = height;
PatchSize = Options.PatchSize > 0
? int.Min(Options.PatchSize, int.Min(width, height))
: int.Clamp(1 << BitOperations.Log2((uint)(int.Min(width, height) / 3)), 8, 256);
// One correlator and blur buffer per patch so the nine patches are analysed in parallel.
_correlators = new PhaseCorrelator[9];
_smooth = new float[9][];
for (var i = 0; i < 9; i++)
{
_correlators[i] = new PhaseCorrelator(PatchSize, PatchSize);
_smooth[i] = new float[PatchSize * PatchSize];
}
_patches = new (int, int)[9];
for (var j = 0; j < 3; j++)
{
for (var i = 0; i < 3; i++)
{
var x = (int)double.Round(width * (2 * i + 1) / 6.0 - PatchSize / 2.0);
var y = (int)double.Round(height * (2 * j + 1) / 6.0 - PatchSize / 2.0);
_patches[j * 3 + i] = (int.Clamp(x, 0, width - PatchSize), int.Clamp(y, 0, height - PatchSize));
}
}
_previousEnergy = new double[9];
_currentEnergy = new double[9];
_previous = ArrayPool.Shared.Rent(width * height);
_current = ArrayPool.Shared.Rent(width * height);
}
public int Width { get; }
public int Height { get; }
public int PatchSize { get; }
public StabilizerOptions Options { get; }
public int FrameCount => _motions.Count;
/// Adds the next frame (its perceptual luminance is analysed) and returns its motion from the previous frame.
public FrameMotion AddFrame(ImageBuffer frame)
{
if (frame.Width != Width || frame.Height != Height) throw new ArgumentException("Frame size differs.", nameof(frame));
PlaneOps.PerceptualLuminance(frame, CurrentPlane.AsSpan(0, Width * Height));
return Analyse();
}
/// Adds the next frame as a perceptual (sRGB-encoded) luminance plane.
public FrameMotion AddFrame(ReadOnlySpan luminance)
{
if (luminance.Length < Width * Height) throw new ArgumentException("Plane is smaller than the frame.", nameof(luminance));
luminance[..(Width * Height)].CopyTo(CurrentPlane);
return Analyse();
}
/// Smooths the accumulated camera path and derives corrections, shake and crop.
public StabilizationResult Solve()
{
var n = _motions.Count;
if (n == 0) throw new InvalidOperationException("No frames were added.");
double cx = (Width - 1) / 2.0, cy = (Height - 1) / 2.0;
// Camera path C_k: frame-0 coordinates → frame-k coordinates, as (tx, ty, angle, ln scale) about the centre.
var path = new Affine2D[n];
var parameters = new double[4][];
for (var p = 0; p < 4; p++) parameters[p] = new double[n];
var current = Affine2D.Identity;
for (var k = 0; k < n; k++)
{
current = _motions[k].Transform * current;
path[k] = current;
var (x, y) = current.Apply(cx, cy);
var angle = current.Angle;
if (k > 0) angle += double.Round((parameters[2][k - 1] - angle) / double.Tau) * double.Tau;
parameters[0][k] = x - cx;
parameters[1][k] = y - cy;
parameters[2][k] = angle;
parameters[3][k] = double.Log(current.Scale);
}
var smoothed = new Affine2D[n];
var corrections = new Affine2D[n];
var series = new double[4][];
for (var p = 0; p < 4; p++) series[p] = PathSmoother.LocalLinear(parameters[p], Options.SmoothingSigma);
for (var k = 0; k < n; k++)
{
smoothed[k] = Affine2D.Translation(cx + series[0][k], cy + series[1][k])
* Affine2D.Similarity(double.Exp(series[3][k]), series[2][k], 0, 0)
* Affine2D.Translation(-cx, -cy);
var correction = smoothed[k] * path[k].Invert();
corrections[k] = correction.IsIdentity(1e-12) ? Affine2D.Identity : correction;
}
var shake = ShakeRms(corrections);
var crop = CropScale(corrections);
return new StabilizationResult(_motions.ToArray(), path, smoothed, corrections, shake, crop, shake > Options.RecommendThreshold);
}
public void Dispose()
{
var previous = Interlocked.Exchange(ref _previous, null);
var current = Interlocked.Exchange(ref _current, null);
if (previous is not null) ArrayPool.Shared.Return(previous);
if (current is not null) ArrayPool.Shared.Return(current);
}
private float[] CurrentPlane => _current ?? throw new ObjectDisposedException(nameof(Stabilizer));
private FrameMotion Analyse()
{
var current = CurrentPlane;
var first = _motions.Count == 0;
var previous = _previous!;
Parallel.For(0, 9, i =>
{
var (x0, y0) = _patches[i];
_currentEnergy[i] = TextureEnergy(current, x0, y0, _smooth[i]);
_results[i] = first || double.Min(_previousEnergy[i], _currentEnergy[i]) < Options.TextureThreshold
? null
: _correlators[i].Correlate(previous, current, Width, x0, y0);
});
// Gathered in patch order, so the fit does not depend on thread scheduling.
var count = 0;
var half = (PatchSize - 1) / 2.0;
for (var i = 0; i < 9; i++)
{
if (_results[i] is not { } r || r.Confidence < Options.MinimumConfidence) continue;
var (x0, y0) = _patches[i];
_matches[count++] = new Match(x0 + half, y0 + half, x0 + half + r.Dx, y0 + half + r.Dy, r.Confidence);
}
var motion = first ? new FrameMotion(Affine2D.Identity, 0, 0) : FitSimilarity(_matches.AsSpan(0, count));
_motions.Add(motion);
(_previous, _current) = (_current, _previous);
_currentEnergy.CopyTo(_previousEnergy, 0);
return motion;
}
/// Noise-compensated mean squared gradient of the blurred patch.
private double TextureEnergy(float[] plane, int x0, int y0, float[] smooth)
{
var p = PatchSize;
var w = Width;
double laplacian = 0;
for (var y = 1; y < p - 1; y++)
{
var row = (y0 + y) * w + x0;
for (var x = 1; x < p - 1; x++)
{
var c = row + x;
float a = plane[c - w - 1], b = plane[c - w], d = plane[c - w + 1];
float e = plane[c - 1], f = plane[c], g = plane[c + 1];
float h = plane[c + w - 1], i = plane[c + w], j = plane[c + w + 1];
smooth[y * p + x] = (a + d + h + j + 2 * (b + e + g + i) + 4 * f) * (1f / 16);
laplacian += float.Abs(a + d + h + j - 2 * (b + e + g + i) + 4 * f);
}
}
var sigma = double.Sqrt(double.Pi / 2) * laplacian / (6.0 * (p - 2) * (p - 2));
double energy = 0;
var samples = 0;
for (var y = 2; y < p - 2; y++)
{
for (var x = 2; x < p - 2; x++)
{
var k = y * p + x;
var gx = 0.5 * (smooth[k + 1] - smooth[k - 1]);
var gy = 0.5 * (smooth[k + p] - smooth[k - p]);
energy += gx * gx + gy * gy;
samples++;
}
}
return samples == 0 ? 0 : energy / samples - NoiseGradientGain * sigma * sigma;
}
private FrameMotion FitSimilarity(ReadOnlySpan matches)
{
var n = matches.Length;
if (n == 0) return new FrameMotion(Affine2D.Identity, 0, 0);
Span inliers = stackalloc bool[n];
Span best = stackalloc bool[n];
var bestCount = 0;
var bestResidual = double.PositiveInfinity;
Span pair = stackalloc bool[n];
for (var a = 0; a < n; a++)
{
for (var b = a + 1; b < n; b++)
{
pair.Clear();
pair[a] = pair[b] = true;
var model = Similarity(matches, pair);
if (double.Abs(model.Angle) > Options.MaxRotation || double.Abs(double.Log(model.Scale)) > Options.MaxScaleChange) continue;
Consider(model, matches, inliers, best, ref bestCount, ref bestResidual);
}
}
// Translation hypotheses, one per vector: covers a single valid patch and sets of implausible pairs.
if (bestCount < 2)
{
foreach (var m in matches) Consider(Affine2D.Translation(m.Qx - m.Px, m.Qy - m.Py), matches, inliers, best, ref bestCount, ref bestResidual);
return new FrameMotion(Translation(matches, best), n, bestCount);
}
return new FrameMotion(Similarity(matches, best), n, bestCount);
}
/// Scores a hypothesis and keeps it when it has more inliers (ties: smaller summed residual).
private void Consider(Affine2D model, ReadOnlySpan matches, Span inliers, Span best, ref int bestCount, ref double bestResidual)
{
var count = 0;
double residual = 0;
for (var i = 0; i < matches.Length; i++)
{
var m = matches[i];
var (x, y) = model.Apply(m.Px, m.Py);
var r = double.Hypot(x - m.Qx, y - m.Qy);
inliers[i] = r <= Options.InlierThreshold;
if (!inliers[i]) continue;
count++;
residual += r;
}
if (count < bestCount || (count == bestCount && residual >= bestResidual)) return;
bestCount = count;
bestResidual = residual;
inliers.CopyTo(best);
}
/// Weighted least-squares similarity q ≈ [a −b; b a]·p + t over the selected correspondences.
private static Affine2D Similarity(ReadOnlySpan matches, ReadOnlySpan selected)
{
double sw = 0, mpx = 0, mpy = 0, mqx = 0, mqy = 0;
for (var i = 0; i < matches.Length; i++)
{
if (!selected[i]) continue;
var m = matches[i];
sw += m.Weight;
mpx += m.Weight * m.Px;
mpy += m.Weight * m.Py;
mqx += m.Weight * m.Qx;
mqy += m.Weight * m.Qy;
}
mpx /= sw;
mpy /= sw;
mqx /= sw;
mqy /= sw;
double dot = 0, cross = 0, norm = 0;
for (var i = 0; i < matches.Length; i++)
{
if (!selected[i]) continue;
var m = matches[i];
double ax = m.Px - mpx, ay = m.Py - mpy, bx = m.Qx - mqx, by = m.Qy - mqy;
dot += m.Weight * (ax * bx + ay * by);
cross += m.Weight * (ax * by - ay * bx);
norm += m.Weight * (ax * ax + ay * ay);
}
if (norm <= 1e-12) return Affine2D.Translation(mqx - mpx, mqy - mpy);
var a = dot / norm;
var b = cross / norm;
return new Affine2D(a, -b, b, a, mqx - (a * mpx - b * mpy), mqy - (b * mpx + a * mpy));
}
private static Affine2D Translation(ReadOnlySpan matches, ReadOnlySpan selected)
{
double sw = 0, tx = 0, ty = 0;
for (var i = 0; i < matches.Length; i++)
{
if (!selected[i]) continue;
var m = matches[i];
sw += m.Weight;
tx += m.Weight * (m.Qx - m.Px);
ty += m.Weight * (m.Qy - m.Py);
}
return sw > 0 ? Affine2D.Translation(tx / sw, ty / sw) : Affine2D.Identity;
}
/// RMS displacement of the patch-grid centres under the corrections.
private double ShakeRms(Affine2D[] corrections)
{
double sum = 0;
var half = (PatchSize - 1) / 2.0;
foreach (var k in corrections)
{
foreach (var (x0, y0) in _patches)
{
var (x, y) = k.Apply(x0 + half, y0 + half);
sum += (x - x0 - half) * (x - x0 - half) + (y - y0 - half) * (y - y0 - half);
}
}
return double.Sqrt(sum / (corrections.Length * _patches.Length));
}
///
/// Largest scale f of a centred, frame-shaped output rectangle whose corners map inside the source under
/// every inverse correction (each corner gives linear bounds on f).
///
private double CropScale(Affine2D[] corrections)
{
double cx = (Width - 1) / 2.0, cy = (Height - 1) / 2.0;
double hx = Width / 2.0, hy = Height / 2.0;
double minX = -0.5, minY = -0.5, maxX = Width - 0.5, maxY = Height - 0.5;
var scale = 1.0;
foreach (var correction in corrections)
{
var inverse = correction.Invert();
var (ox, oy) = inverse.Apply(cx, cy);
if (ox < minX || ox > maxX || oy < minY || oy > maxY) return 0;
for (var corner = 0; corner < 4; corner++)
{
var (dx, dy) = inverse.ApplyVector((corner & 1) == 0 ? -hx : hx, (corner & 2) == 0 ? -hy : hy);
scale = Limit(scale, ox, dx, minX, maxX);
scale = Limit(scale, oy, dy, minY, maxY);
}
}
return double.Clamp(scale, 0, 1);
static double Limit(double f, double origin, double direction, double min, double max)
{
if (direction > 0) return double.Min(f, (max - origin) / direction);
if (direction < 0) return double.Min(f, (min - origin) / direction);
return f;
}
}
/// Patch centre in the previous frame (P) and where its content moved in the current one (Q).
private readonly record struct Match(double Px, double Py, double Qx, double Qy, double Weight);
}