diff --git a/src/Titano.Core/Motion/GrayPyramid.cs b/src/Titano.Core/Motion/GrayPyramid.cs new file mode 100644 index 00000000..f8abb9b3 --- /dev/null +++ b/src/Titano.Core/Motion/GrayPyramid.cs @@ -0,0 +1,144 @@ +using System.Buffers; +using System.Numerics; +using Titano.Core.Imaging; + +namespace Titano.Core.Motion; + +/// +/// Gaussian pyramid of a single-channel plane (Burt & Adelson 1983): each level is the previous one +/// filtered with the 5-tap binomial [1 4 6 4 1]/16 (replicated edges) and decimated by 2, keeping even +/// samples, so level-ℓ pixel k sits at full-resolution coordinate k·2^ℓ. Planes are pooled and reused +/// by every . +/// +public sealed class GrayPyramid : IDisposable +{ + private readonly float[][] _levels; + private readonly int[] _widths; + private readonly int[] _heights; + private float[]? _scratch; + + public GrayPyramid(int width, int height, int levels) + { + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(width); + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(height); + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(levels); + Width = width; + Height = height; + Levels = levels; + _levels = new float[levels][]; + _widths = new int[levels]; + _heights = new int[levels]; + for (var l = 0; l < levels; l++) + { + _widths[l] = l == 0 ? width : (_widths[l - 1] + 1) / 2; + _heights[l] = l == 0 ? height : (_heights[l - 1] + 1) / 2; + _levels[l] = ArrayPool.Shared.Rent(_widths[l] * _heights[l]); + } + _scratch = ArrayPool.Shared.Rent(int.Max(1, _widths[int.Min(1, levels - 1)] * height)); + } + + public int Width { get; } + + public int Height { get; } + + public int Levels { get; } + + /// Levels needed until the smaller side would drop below , capped. + public static int LevelsFor(int width, int height, int minimumSize = 16, int maximumLevels = 8) + { + var levels = 1; + while (levels < maximumLevels && (int.Min(width, height) + 1) / 2 >= minimumSize) + { + width = (width + 1) / 2; + height = (height + 1) / 2; + levels++; + } + return levels; + } + + public int LevelWidth(int level) => _widths[level]; + + public int LevelHeight(int level) => _heights[level]; + + /// Row-major samples of one level. + public ReadOnlySpan Level(int level) => LevelArray(level).AsSpan(0, _widths[level] * _heights[level]); + + internal float[] LevelArray(int level) + { + ObjectDisposedException.ThrowIf(_scratch is null, this); + return _levels[level]; + } + + /// Builds the pyramid from a plane of × samples. + public void Build(ReadOnlySpan plane) + { + if (plane.Length < Width * Height) throw new ArgumentException("Plane is smaller than the pyramid base.", nameof(plane)); + plane[..(Width * Height)].CopyTo(LevelArray(0)); + BuildUpperLevels(); + } + + /// Builds the pyramid from the perceptual (sRGB-encoded) luminance of an image. + public void Build(ImageBuffer image) + { + if (image.Width != Width || image.Height != Height) throw new ArgumentException("Image size differs from the pyramid.", nameof(image)); + PlaneOps.PerceptualLuminance(image, LevelArray(0).AsSpan(0, Width * Height)); + BuildUpperLevels(); + } + + public void Dispose() + { + var scratch = Interlocked.Exchange(ref _scratch, null); + if (scratch is null) return; + ArrayPool.Shared.Return(scratch); + foreach (var level in _levels) ArrayPool.Shared.Return(level); + } + + private void BuildUpperLevels() + { + for (var l = 1; l < Levels; l++) Downsample(_levels[l - 1], _widths[l - 1], _heights[l - 1], _levels[l], _widths[l], _heights[l], _scratch!); + } + + private static void Downsample(float[] source, int width, int height, float[] destination, int halfWidth, int halfHeight, float[] scratch) + { + // Horizontal: filter and keep even columns (scratch is halfWidth × height). + PlaneOps.ForRows(height, width, y => + { + var row = source.AsSpan(y * width, width); + var output = scratch.AsSpan(y * halfWidth, halfWidth); + var last = width - 1; + for (var k = 0; k < halfWidth; k++) + { + var x = 2 * k; + output[k] = (row[int.Max(x - 2, 0)] + row[int.Min(x + 2, last)] + + 4 * (row[int.Max(x - 1, 0)] + row[int.Min(x + 1, last)]) + + 6 * row[int.Min(x, last)]) * (1f / 16); + } + }); + + // Vertical: five rows combined per output row, vectorised across columns. + PlaneOps.ForRows(halfHeight, halfWidth, k => + { + var y = 2 * k; + var last = height - 1; + var r0 = scratch.AsSpan(int.Max(y - 2, 0) * halfWidth, halfWidth); + var r1 = scratch.AsSpan(int.Max(y - 1, 0) * halfWidth, halfWidth); + var r2 = scratch.AsSpan(int.Min(y, last) * halfWidth, halfWidth); + var r3 = scratch.AsSpan(int.Min(y + 1, last) * halfWidth, halfWidth); + var r4 = scratch.AsSpan(int.Min(y + 2, last) * halfWidth, halfWidth); + var output = destination.AsSpan(k * halfWidth, halfWidth); + var i = 0; + if (Vector.IsHardwareAccelerated) + { + var vc = Vector.Count; + for (; i <= halfWidth - vc; i += vc) + { + var sum = new Vector(r0[i..]) + new Vector(r4[i..]) + + 4 * (new Vector(r1[i..]) + new Vector(r3[i..])) + + 6 * new Vector(r2[i..]); + (sum * (1f / 16)).CopyTo(output[i..]); + } + } + for (; i < halfWidth; i++) output[i] = (r0[i] + r4[i] + 4 * (r1[i] + r3[i]) + 6 * r2[i]) * (1f / 16); + }); + } +} diff --git a/src/Titano.Core/Motion/MotionField.cs b/src/Titano.Core/Motion/MotionField.cs new file mode 100644 index 00000000..bde6163f --- /dev/null +++ b/src/Titano.Core/Motion/MotionField.cs @@ -0,0 +1,154 @@ +using System.Buffers; + +namespace Titano.Core.Motion; + +/// Summary of a motion field's vector magnitudes (pixels per frame). +public readonly record struct MotionStatistics( + double MeanMagnitude, + double MedianMagnitude, + double P95Magnitude, + double MaxMagnitude, + double MeanU, + double MeanV, + double ReliableFraction); + +/// +/// Dense motion vectors (full-resolution pixels per frame) on a regular grid: node (i, j) describes the +/// pixel at (i·Step, j·Step). A vector (u, v) at p means the content at p in the first frame is found at +/// p + (u, v) in the second. holds the forward–backward consistency error in pixels +/// (NaN when not checked). Storage is pooled; dispose the field when done. +/// +public sealed class MotionField : IDisposable +{ + private float[]? _u; + private float[]? _v; + private float[]? _error; + + public MotionField(int imageWidth, int imageHeight, int step) + { + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(imageWidth); + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(imageHeight); + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(step); + ImageWidth = imageWidth; + ImageHeight = imageHeight; + Step = step; + Columns = (imageWidth - 1) / step + 1; + Rows = (imageHeight - 1) / step + 1; + var n = Columns * Rows; + _u = ArrayPool.Shared.Rent(n); + _v = ArrayPool.Shared.Rent(n); + _error = ArrayPool.Shared.Rent(n); + U.Clear(); + V.Clear(); + Error.Fill(float.NaN); + } + + public int ImageWidth { get; } + + public int ImageHeight { get; } + + /// Grid spacing in full-resolution pixels. + public int Step { get; } + + public int Columns { get; } + + public int Rows { get; } + + public int Count => Columns * Rows; + + /// Horizontal components, row-major over the grid. + public Span U => Storage(_u); + + /// Vertical components, row-major over the grid. + public Span V => Storage(_v); + + /// Forward–backward error |w(p) + w'(p + w(p))| in pixels; NaN when unchecked. + public Span Error => Storage(_error); + + /// True when has been filled by a forward–backward check. + public bool HasConsistency { get; internal set; } + + /// Bilinearly interpolated vector at a full-resolution pixel-centre position (clamped to the grid). + public (float U, float V) Sample(double x, double y) + { + var gx = (float)(x / Step); + var gy = (float)(y / Step); + return (PlaneOps.Bilinear(U, Columns, Rows, gx, gy), PlaneOps.Bilinear(V, Columns, Rows, gx, gy)); + } + + /// Bilinearly interpolated forward–backward error (NaN when unchecked). + public float SampleError(double x, double y) => + HasConsistency ? PlaneOps.Bilinear(Error, Columns, Rows, (float)(x / Step), (float)(y / Step)) : float.NaN; + + /// Counts of vector magnitudes in equal bins over [0, max]; larger values land in the last bin. + public int[] MagnitudeHistogram(int bins, double maxMagnitude) + { + ArgumentOutOfRangeException.ThrowIfNegativeOrZero(bins); + if (!(maxMagnitude > 0)) throw new ArgumentOutOfRangeException(nameof(maxMagnitude)); + var histogram = new int[bins]; + var u = U; + var v = V; + var scale = bins / maxMagnitude; + for (var i = 0; i < u.Length; i++) + { + var m = double.Sqrt((double)u[i] * u[i] + (double)v[i] * v[i]); + histogram[int.Min(bins - 1, (int)(m * scale))]++; + } + return histogram; + } + + /// Magnitude statistics; vectors with a forward–backward error above px count as unreliable. + public MotionStatistics Statistics(double reliableError = 1.0) + { + var n = Count; + var magnitudes = ArrayPool.Shared.Rent(n); + try + { + var u = U; + var v = V; + var error = Error; + double sum = 0, su = 0, sv = 0, max = 0; + var reliable = 0; + for (var i = 0; i < n; i++) + { + var m = float.Sqrt(u[i] * u[i] + v[i] * v[i]); + magnitudes[i] = m; + sum += m; + su += u[i]; + sv += v[i]; + max = double.Max(max, m); + if (!HasConsistency || error[i] <= reliableError) reliable++; + } + var span = magnitudes.AsSpan(0, n); + var median = PlaneOps.Median(span); + var p95 = PlaneOps.Select(span, int.Min(n - 1, (int)double.Ceiling(0.95 * n) - 1)); + return new MotionStatistics(sum / n, median, p95, max, su / n, sv / n, reliable / (double)n); + } + finally + { + ArrayPool.Shared.Return(magnitudes); + } + } + + /// Sets every vector to (u, v). + public void Fill(float u, float v) + { + U.Fill(u); + V.Fill(v); + } + + public void Dispose() + { + Return(ref _u); + Return(ref _v); + Return(ref _error); + } + + private Span Storage(float[]? array) => (array ?? throw new ObjectDisposedException(nameof(MotionField))).AsSpan(0, Count); + + private static void Return(ref float[]? array) + { + var a = Interlocked.Exchange(ref array, null); + if (a is not null) ArrayPool.Shared.Return(a); + } +} diff --git a/src/Titano.Core/Motion/OpticalFlowEngine.cs b/src/Titano.Core/Motion/OpticalFlowEngine.cs new file mode 100644 index 00000000..d3e1070c --- /dev/null +++ b/src/Titano.Core/Motion/OpticalFlowEngine.cs @@ -0,0 +1,332 @@ +using System.Buffers; +using Titano.Core.Imaging; + +namespace Titano.Core.Motion; + +/// Speed/quality trade-off of . +public enum FlowQuality +{ + /// Vectors every 4 px, few iterations: previews and analysis histograms. + Draft, + + /// Vectors every 2 px: motion blur and analysis. + Balanced, + + /// Per-pixel vectors, more iterations and a 5×5 median: frame interpolation. + Best, +} + +/// Parameters of the dense pyramidal Lucas–Kanade solver. +public sealed record OpticalFlowOptions +{ + /// Upper bound on pyramid levels (the coarsest level keeps ≥ 16 px on its short side). + public int MaxLevels { get; init; } = 5; + + /// Finest level solved; the output grid step is 2^FinestLevel pixels. + public int FinestLevel { get; init; } = 1; + + /// Warp–solve iterations per level. + public int Iterations { get; init; } = 4; + + /// Lucas–Kanade integration window radius (box of (2r+1)² samples) at every level. + public int WindowRadius { get; init; } = 3; + + /// Median filter radius applied to the field after each level (0 = off). + public int MedianRadius { get; init; } = 1; + + /// Tikhonov term added to the structure tensor diagonal (perceptual luminance² per px²). + public float Regularization { get; init; } = 1e-5f; + + public static OpticalFlowOptions For(FlowQuality quality) => quality switch + { + FlowQuality.Draft => new OpticalFlowOptions { MaxLevels = 4, FinestLevel = 2, Iterations = 3 }, + FlowQuality.Balanced => new OpticalFlowOptions(), + FlowQuality.Best => new OpticalFlowOptions { MaxLevels = 6, FinestLevel = 0, Iterations = 5, WindowRadius = 4, MedianRadius = 2 }, + _ => throw new ArgumentOutOfRangeException(nameof(quality)), + }; +} + +/// +/// Dense optical flow by pyramidal Lucas–Kanade (Lucas & Kanade 1981; Bouguet 2000) solved for a window +/// around every pixel of each level: both frames get a σ = 1 binomial blur, box-window structure tensors of +/// the first frame's gradients are inverted once per level (inverse-compositional form), and each iteration +/// warps the second frame by the current flow (bilinear) and re-solves every window with a first-order +/// correction for its neighbours' flows; the field is median filtered between levels (Sun, Roth & Black +/// 2010). Chosen over Farnebäck polynomial expansion because every step is a separable box filter or a +/// per-pixel 2×2 solve that vectorises and parallelises cleanly, with equal sub-pixel accuracy on smooth +/// motion. Forward–backward consistency (Sundaram et al. 2010) marks occlusions. One engine holds reusable +/// workspaces, so it must not be shared between threads. +/// +public sealed class OpticalFlowEngine : IDisposable +{ + private const float MaxUpdate = 2f; + + private GrayPyramid? _first; + private GrayPyramid? _second; + private float[][] _work = []; + private int _capacity; + + public OpticalFlowEngine(OpticalFlowOptions? options = null) => Options = options ?? new OpticalFlowOptions(); + + public OpticalFlowEngine(FlowQuality quality) : this(OpticalFlowOptions.For(quality)) + { + } + + public OpticalFlowOptions Options { get; } + + /// Flow from to (perceptual luminance of both). + public MotionField Compute(ImageBuffer from, ImageBuffer to, CancellationToken cancellationToken = default) + { + BuildPyramids(from, to); + return Compute(_first!, _second!, cancellationToken); + } + + /// Flow between two prebuilt pyramids (lets callers reuse each frame's pyramid for two pairs). + public MotionField Compute(GrayPyramid from, GrayPyramid to, CancellationToken cancellationToken = default) + { + if (from.Width != to.Width || from.Height != to.Height || from.Levels != to.Levels) + throw new ArgumentException("Pyramids differ in size or depth.", nameof(to)); + var coarsest = int.Min(from.Levels, Options.MaxLevels) - 1; + var finest = int.Clamp(Options.FinestLevel, 0, coarsest); + EnsureWorkspace(from.LevelWidth(finest) * from.LevelHeight(finest)); + + float[] u = _work[0], v = _work[1]; + for (var level = coarsest; level >= finest; level--) + { + cancellationToken.ThrowIfCancellationRequested(); + var w = from.LevelWidth(level); + var h = from.LevelHeight(level); + if (level == coarsest) + { + Array.Clear(u, 0, w * h); + Array.Clear(v, 0, w * h); + } + else + { + var (nu, nv) = u == _work[0] ? (_work[2], _work[3]) : (_work[0], _work[1]); + Upsample(u, nu, from.LevelWidth(level + 1), from.LevelHeight(level + 1), w, h); + Upsample(v, nv, from.LevelWidth(level + 1), from.LevelHeight(level + 1), w, h); + (u, v) = (nu, nv); + } + SolveLevel(from.LevelArray(level), to.LevelArray(level), w, h, u, v, cancellationToken); + } + + var step = 1 << finest; + var field = new MotionField(from.Width, from.Height, step); + var n = field.Count; + var fu = field.U; + var fv = field.V; + for (var i = 0; i < n; i++) + { + fu[i] = u[i] * step; + fv[i] = v[i] * step; + } + return field; + } + + /// Forward and backward flow with their forward–backward consistency errors filled in. + public (MotionField Forward, MotionField Backward) ComputeBidirectional(ImageBuffer from, ImageBuffer to, CancellationToken cancellationToken = default) + { + BuildPyramids(from, to); + return ComputeBidirectional(_first!, _second!, cancellationToken); + } + + /// + public (MotionField Forward, MotionField Backward) ComputeBidirectional(GrayPyramid from, GrayPyramid to, CancellationToken cancellationToken = default) + { + var forward = Compute(from, to, cancellationToken); + try + { + var backward = Compute(to, from, cancellationToken); + CheckConsistency(forward, backward); + return (forward, backward); + } + catch + { + forward.Dispose(); + throw; + } + } + + /// + /// Fills both fields' with |w(p) + w'(p + w(p))|; vectors leaving the + /// frame get +∞ (unverifiable, treated as occluded). + /// + public static void CheckConsistency(MotionField forward, MotionField backward) + { + if (forward.ImageWidth != backward.ImageWidth || forward.ImageHeight != backward.ImageHeight) + throw new ArgumentException("Fields describe different image sizes.", nameof(backward)); + Fill(forward, backward); + Fill(backward, forward); + forward.HasConsistency = backward.HasConsistency = true; + + static void Fill(MotionField field, MotionField other) + { + var u = field.U; + var v = field.V; + var error = field.Error; + var maxX = field.ImageWidth - 0.5; + var maxY = field.ImageHeight - 0.5; + for (var j = 0; j < field.Rows; j++) + { + for (var i = 0; i < field.Columns; i++) + { + var k = j * field.Columns + i; + var x = i * field.Step + (double)u[k]; + var y = j * field.Step + (double)v[k]; + if (x < -0.5 || y < -0.5 || x > maxX || y > maxY) + { + error[k] = float.PositiveInfinity; + continue; + } + var (bu, bv) = other.Sample(x, y); + error[k] = float.Hypot(u[k] + bu, v[k] + bv); + } + } + } + } + + public void Dispose() + { + _first?.Dispose(); + _second?.Dispose(); + _first = _second = null; + foreach (var a in _work) ArrayPool.Shared.Return(a); + _work = []; + _capacity = 0; + } + + private void BuildPyramids(ImageBuffer from, ImageBuffer to) + { + if (from.Width != to.Width || from.Height != to.Height) throw new ArgumentException("Frames differ in size.", nameof(to)); + if (_first is null || _first.Width != from.Width || _first.Height != from.Height) + { + _first?.Dispose(); + _second?.Dispose(); + var levels = GrayPyramid.LevelsFor(from.Width, from.Height, 16, Options.MaxLevels); + _first = new GrayPyramid(from.Width, from.Height, levels); + _second = new GrayPyramid(from.Width, from.Height, levels); + } + _first.Build(from); + _second!.Build(to); + } + + private void EnsureWorkspace(int length) + { + if (length <= _capacity) return; + foreach (var a in _work) ArrayPool.Shared.Return(a); + // u, v, u', v', Ix, Iy, H11, H12, H22, Bx, By, scratch, smoothed first, smoothed second. + _work = new float[14][]; + for (var i = 0; i < _work.Length; i++) _work[i] = ArrayPool.Shared.Rent(length); + _capacity = length; + } + + private void SolveLevel(float[] i0, float[] i1, int w, int h, float[] u, float[] v, CancellationToken cancellationToken) + { + var (spareU, spareV) = u == _work[0] ? (_work[2], _work[3]) : (_work[0], _work[1]); + float[] ix = _work[4], iy = _work[5], h11 = _work[6], h12 = _work[7], h22 = _work[8], bx = _work[9], by = _work[10], scratch = _work[11]; + var radius = Options.WindowRadius; + var lambda = Options.Regularization; + + // Central differences under-estimate high-frequency derivatives relative to how the bilinearly warped + // frame responds to a flow change (ratio 1/cos πf), which over-shoots the Gauss–Newton step beyond 2× + // above f ≈ 1/3 cycles/px and makes the iteration diverge. A σ = 1 blur of both frames removes that + // band; being the same translation-invariant filter on both, it does not bias the solution. + PlaneOps.Binomial5(i0, _work[12], scratch, w, h); + PlaneOps.Binomial5(i1, _work[13], scratch, w, h); + i0 = _work[12]; + i1 = _work[13]; + + // Gradients of the first frame and their structure-tensor products. + PlaneOps.ForRows(h, w, y => + { + var row = y * w; + var up = int.Max(y - 1, 0) * w; + var down = int.Min(y + 1, h - 1) * w; + for (var x = 0; x < w; x++) + { + var gx = 0.5f * (i0[row + int.Min(x + 1, w - 1)] - i0[row + int.Max(x - 1, 0)]); + var gy = 0.5f * (i0[down + x] - i0[up + x]); + ix[row + x] = gx; + iy[row + x] = gy; + h11[row + x] = gx * gx; + h12[row + x] = gx * gy; + h22[row + x] = gy * gy; + } + }); + PlaneOps.BoxFilter(h11, h11, scratch, w, h, radius); + PlaneOps.BoxFilter(h12, h12, scratch, w, h, radius); + PlaneOps.BoxFilter(h22, h22, scratch, w, h, radius); + PlaneOps.ForRows(h, w, y => + { + for (int x = 0, k = y * w; x < w; x++, k++) + { + var a = h11[k] + lambda; + var b = h12[k]; + var c = h22[k] + lambda; + var r = 1f / (a * c - b * b); + h11[k] = c * r; + h12[k] = -b * r; + h22[k] = a * r; + } + }); + + for (var iteration = 0; iteration < Options.Iterations; iteration++) + { + cancellationToken.ThrowIfCancellationRequested(); + // Each window solves for one flow v: min Σ_y (I1w(y) + g(y)·(v − u(y)) − I0(y))², i.e. the classic + // Lucas–Kanade window with a first-order correction for the neighbours' own flows. Unlike the + // per-pixel update u −= G⁻¹·box(g·It), this does not feed the current error back through + // ill-conditioned (aperture) windows, so it cannot diverge. Samples warped outside the frame + // vote for their current flow (It = 0). + PlaneOps.ForRows(h, w, y => + { + var span = i1.AsSpan(0, w * h); + for (int x = 0, k = y * w; x < w; x++, k++) + { + var px = x + u[k]; + var py = y + v[k]; + var it = px >= 0 && py >= 0 && px <= w - 1 && py <= h - 1 + ? PlaneOps.Bilinear(span, w, h, px, py) - i0[k] + : 0f; + var r = ix[k] * u[k] + iy[k] * v[k] - it; + bx[k] = ix[k] * r; + by[k] = iy[k] * r; + } + }); + PlaneOps.BoxFilter(bx, bx, scratch, w, h, radius); + PlaneOps.BoxFilter(by, by, scratch, w, h, radius); + PlaneOps.ForRows(h, w, y => + { + for (int x = 0, k = y * w; x < w; x++, k++) + { + // (G + λI)⁻¹ (box(q) + λu): the regulariser keeps textureless windows at their current flow. + var qx = bx[k] + lambda * u[k]; + var qy = by[k] + lambda * v[k]; + var nu = h11[k] * qx + h12[k] * qy; + var nv = h12[k] * qx + h22[k] * qy; + u[k] += float.Clamp(nu - u[k], -MaxUpdate, MaxUpdate); + v[k] += float.Clamp(nv - v[k], -MaxUpdate, MaxUpdate); + } + }); + } + + if (Options.MedianRadius > 0) + { + PlaneOps.MedianFilter(u, spareU, w, h, Options.MedianRadius); + PlaneOps.MedianFilter(v, spareV, w, h, Options.MedianRadius); + Array.Copy(spareU, u, w * h); + Array.Copy(spareV, v, w * h); + } + } + + /// Doubles the resolution and the magnitude of a level's flow (level-ℓ pixel k ↔ level-(ℓ−1) pixel 2k). + private static void Upsample(float[] coarse, float[] fine, int cw, int ch, int w, int h) + { + PlaneOps.ForRows(h, w, y => + { + var span = coarse.AsSpan(0, cw * ch); + for (var x = 0; x < w; x++) fine[y * w + x] = 2 * PlaneOps.Bilinear(span, cw, ch, 0.5f * x, 0.5f * y); + }); + } +} diff --git a/src/Titano.Core/Motion/PlaneOps.cs b/src/Titano.Core/Motion/PlaneOps.cs new file mode 100644 index 00000000..b93b65ef --- /dev/null +++ b/src/Titano.Core/Motion/PlaneOps.cs @@ -0,0 +1,222 @@ +using System.Numerics; +using System.Runtime.CompilerServices; +using Titano.Core.Imaging; + +namespace Titano.Core.Motion; + +/// Row-parallel kernels on single-channel float planes shared by the motion modules. +internal static class PlaneOps +{ + /// Below this many pixels per pass the Parallel.For overhead outweighs the work. + public const int ParallelThreshold = 1 << 15; + + /// Runs for every row, in parallel when the plane is large. + public static void ForRows(int height, int width, Action body) + { + if ((long)height * width < ParallelThreshold) + { + for (var y = 0; y < height; y++) body(y); + } + else + { + Parallel.For(0, height, body); + } + } + + /// + /// Perceptually uniform luminance (sRGB-encoded Rec.709 Y, 0..1). Motion estimation works on it so that + /// shadows carry texture weight comparable to highlights, unlike linear light. + /// + public static void PerceptualLuminance(ImageBuffer image, Span destination) + { + ColorSpace.Luminance(image, destination); + for (var i = 0; i < destination.Length; i++) destination[i] = ColorSpace.EncodeFast(destination[i]); + } + + /// Bilinear sample with edge clamping at pixel-centre coordinates. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float Bilinear(ReadOnlySpan plane, int width, int height, float x, float y) + { + x = float.Clamp(x, 0, width - 1); + y = float.Clamp(y, 0, height - 1); + var x0 = int.Min((int)x, width - 2 < 0 ? 0 : width - 2); + var y0 = int.Min((int)y, height - 2 < 0 ? 0 : height - 2); + var fx = x - x0; + var fy = y - y0; + var x1 = int.Min(x0 + 1, width - 1); + var y1 = int.Min(y0 + 1, height - 1); + var r0 = y0 * width; + var r1 = y1 * width; + var top = plane[r0 + x0] + (plane[r0 + x1] - plane[r0 + x0]) * fx; + var bottom = plane[r1 + x0] + (plane[r1 + x1] - plane[r1 + x0]) * fx; + return top + (bottom - top) * fy; + } + + /// + /// Mean over a (2r+1)² box with replicated edges, by running sums: horizontal pass per row, then a + /// vertical pass that keeps Vector<float> column sums for blocks of columns. + /// + public static void BoxFilter(float[] source, float[] destination, float[] scratch, int width, int height, int radius) + { + var inv = 1f / (2 * radius + 1); + ForRows(height, width, y => + { + var row = source.AsSpan(y * width, width); + var output = scratch.AsSpan(y * width, width); + float sum = 0; + for (var k = -radius; k <= radius; k++) sum += row[int.Clamp(k, 0, width - 1)]; + for (var x = 0; x < width; x++) + { + output[x] = sum * inv; + sum += row[int.Min(x + radius + 1, width - 1)] - row[int.Max(x - radius, 0)]; + } + }); + + const int Block = 256; + var blocks = (width + Block - 1) / Block; + void Columns(int b) + { + var x0 = b * Block; + var n = int.Min(Block, width - x0); + Span acc = stackalloc float[Block]; + acc = acc[..n]; + acc.Clear(); + for (var k = -radius; k <= radius; k++) Add(acc, scratch.AsSpan(int.Clamp(k, 0, height - 1) * width + x0, n), 1f); + for (var y = 0; y < height; y++) + { + Scale(acc, destination.AsSpan(y * width + x0, n), inv); + Add(acc, scratch.AsSpan(int.Min(y + radius + 1, height - 1) * width + x0, n), 1f); + Add(acc, scratch.AsSpan(int.Max(y - radius, 0) * width + x0, n), -1f); + } + } + if ((long)width * height < ParallelThreshold || blocks == 1) + { + for (var b = 0; b < blocks; b++) Columns(b); + } + else + { + Parallel.For(0, blocks, Columns); + } + } + + /// Separable 5-tap binomial [1 4 6 4 1]/16 blur (σ = 1 px) with replicated edges; may run in place. + public static void Binomial5(float[] source, float[] destination, float[] scratch, int width, int height) + { + ForRows(height, width, y => + { + var row = source.AsSpan(y * width, width); + var output = scratch.AsSpan(y * width, width); + var last = width - 1; + for (var x = 0; x < width; x++) + { + output[x] = (row[int.Max(x - 2, 0)] + row[int.Min(x + 2, last)] + + 4 * (row[int.Max(x - 1, 0)] + row[int.Min(x + 1, last)]) + 6 * row[x]) * (1f / 16); + } + }); + ForRows(height, width, y => + { + var last = height - 1; + var r0 = scratch.AsSpan(int.Max(y - 2, 0) * width, width); + var r1 = scratch.AsSpan(int.Max(y - 1, 0) * width, width); + var r2 = scratch.AsSpan(y * width, width); + var r3 = scratch.AsSpan(int.Min(y + 1, last) * width, width); + var r4 = scratch.AsSpan(int.Min(y + 2, last) * width, width); + var output = destination.AsSpan(y * width, width); + var i = 0; + if (Vector.IsHardwareAccelerated) + { + for (; i <= width - Vector.Count; i += Vector.Count) + { + var sum = new Vector(r0[i..]) + new Vector(r4[i..]) + + 4 * (new Vector(r1[i..]) + new Vector(r3[i..])) + + 6 * new Vector(r2[i..]); + (sum * (1f / 16)).CopyTo(output[i..]); + } + } + for (; i < width; i++) output[i] = (r0[i] + r4[i] + 4 * (r1[i] + r3[i]) + 6 * r2[i]) * (1f / 16); + }); + } + + /// Median over a (2r+1)² window with replicated edges. + public static void MedianFilter(float[] source, float[] destination, int width, int height, int radius) + { + ForRows(height, width, y => + { + var size = 2 * radius + 1; + Span window = stackalloc float[size * size]; + for (var x = 0; x < width; x++) + { + var n = 0; + for (var dy = -radius; dy <= radius; dy++) + { + var row = int.Clamp(y + dy, 0, height - 1) * width; + for (var dx = -radius; dx <= radius; dx++) window[n++] = source[row + int.Clamp(x + dx, 0, width - 1)]; + } + destination[y * width + x] = Select(window, n / 2); + } + }); + } + + /// Exact median (mean of the two middle values for even counts); reorders . + public static float Median(Span values) + { + var n = values.Length; + var upper = Select(values, n / 2); + if ((n & 1) != 0) return upper; + // After selection everything left of n/2 is ≤ upper: the lower middle is their maximum. + var lower = values[0]; + for (var i = 1; i < n / 2; i++) lower = float.Max(lower, values[i]); + return 0.5f * (lower + upper); + } + + /// k-th smallest value by Wirth/Hoare selection (in place, O(n) expected). + public static float Select(Span values, int k) + { + int left = 0, right = values.Length - 1; + while (left < right) + { + var pivot = values[k]; + int i = left, j = right; + do + { + while (values[i] < pivot) i++; + while (pivot < values[j]) j--; + if (i <= j) + { + (values[i], values[j]) = (values[j], values[i]); + i++; + j--; + } + } + while (i <= j); + if (j < k) left = i; + if (k < i) right = j; + } + return values[k]; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void Add(Span acc, ReadOnlySpan row, float sign) + { + var i = 0; + if (Vector.IsHardwareAccelerated) + { + var s = new Vector(sign); + for (; i <= acc.Length - Vector.Count; i += Vector.Count) + (new Vector(acc[i..]) + new Vector(row[i..]) * s).CopyTo(acc[i..]); + } + for (; i < acc.Length; i++) acc[i] += row[i] * sign; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static void Scale(ReadOnlySpan acc, Span output, float factor) + { + var i = 0; + if (Vector.IsHardwareAccelerated) + { + for (; i <= acc.Length - Vector.Count; i += Vector.Count) + (new Vector(acc[i..]) * factor).CopyTo(output[i..]); + } + for (; i < acc.Length; i++) output[i] = acc[i] * factor; + } +} diff --git a/tests/Titano.Core.Tests/Motion/OpticalFlowTests.cs b/tests/Titano.Core.Tests/Motion/OpticalFlowTests.cs new file mode 100644 index 00000000..e9a0518d --- /dev/null +++ b/tests/Titano.Core.Tests/Motion/OpticalFlowTests.cs @@ -0,0 +1,155 @@ +using Titano.Core.Imaging; +using Titano.Core.Imaging.Geometry; +using Titano.Core.Motion; +using Titano.Testing; + +namespace Titano.Core.Tests.Motion; + +public sealed class OpticalFlowTests +{ + private const int Base = 256; + private const int Frame = 192; + private const int Margin = 16; + + private readonly TestContext _context; + + public OpticalFlowTests(TestContext context) => _context = context; + + [Theory] + [Case(FlowQuality.Draft)] + [Case(FlowQuality.Balanced)] + [Case(FlowQuality.Best)] + public void TranslationEndPointErrorBelowAThirdOfAPixel(FlowQuality quality) + { + const double dx = 2.3, dy = -1.7; + var texture = SyntheticImages.Natural(Base, seed: 11); + using var a = SyntheticImages.ToRgb(SyntheticImages.Crop(texture, Base, 32, 32, Frame, Frame), Frame, Frame); + using var b = SyntheticImages.ToRgb(SyntheticImages.Crop(SyntheticImages.Shift(texture, Base, dx, dy), Base, 32, 32, Frame, Frame), Frame, Frame); + using var engine = new OpticalFlowEngine(quality); + using var field = engine.Compute(a, b); + var epe = MeanEndPointError(field, (_, _) => (dx, dy)); + _context.Log($"{quality}: step {field.Step}, mean EPE {epe:F4} px"); + Assert.Less(epe, 0.3); + } + + [Test] + public void RotationEndPointErrorBelowOnePixel() + { + var angle = 3.0 * double.Pi / 180; + var texture = SyntheticImages.Natural(Base, seed: 5); + using var baseImage = SyntheticImages.ToRgb(SyntheticImages.Crop(texture, Base, 0, 0, Base, Base), Base, Base); + using var rotated = new ImageBuffer(Base, Base); + const double c = (Base - 1) / 2.0; + // rotated(x) = base(R⁻¹(x − c) + c): content turns by +angle about the centre. + GeometryStage.Warp(baseImage, rotated, Affine2D.About(Affine2D.Rotation(-angle), c, c)); + using var a = Crop(baseImage, 32); + using var b = Crop(rotated, 32); + using var engine = new OpticalFlowEngine(FlowQuality.Best); + using var field = engine.Compute(a, b); + var truth = Affine2D.About(Affine2D.Rotation(angle), c - 32, c - 32); + var epe = MeanEndPointError(field, (x, y) => + { + var (tx, ty) = truth.Apply(x, y); + return (tx - x, ty - y); + }); + _context.Log($"rotation 3°: mean EPE {epe:F4} px"); + Assert.Less(epe, 1.0); + } + + [Test] + public void ForwardBackwardErrorIsSmallForConsistentMotion() + { + var texture = SyntheticImages.Natural(Base, seed: 2); + using var a = SyntheticImages.ToRgb(SyntheticImages.Crop(texture, Base, 32, 32, Frame, Frame), Frame, Frame); + using var b = SyntheticImages.ToRgb(SyntheticImages.Crop(SyntheticImages.Shift(texture, Base, -1.5, 0.75), Base, 32, 32, Frame, Frame), Frame, Frame); + using var engine = new OpticalFlowEngine(FlowQuality.Balanced); + var (forward, backward) = engine.ComputeBidirectional(a, b); + using (forward) + using (backward) + { + Assert.True(forward.HasConsistency); + var stats = forward.Statistics(reliableError: 0.5); + _context.Log($"reliable {stats.ReliableFraction:P1}, mean |w| {stats.MeanMagnitude:F3}"); + Assert.Greater(stats.ReliableFraction, 0.85); + Assert.Approximately(1.677, stats.MedianMagnitude, 0.1); + Assert.Less(forward.SampleError(96, 96), 0.2); + var (bu, bv) = backward.Sample(96, 96); + Assert.Within(1.5, -0.75, bu, bv, 0.2); + } + } + + [Test] + public void FieldSamplingHistogramAndStatistics() + { + using var field = new MotionField(9, 5, 4); + Assert.Equal(3, field.Columns); + Assert.Equal(2, field.Rows); + for (var j = 0; j < field.Rows; j++) + for (var i = 0; i < field.Columns; i++) + { + field.U[j * field.Columns + i] = i; + field.V[j * field.Columns + i] = 2 * j; + } + var (u, v) = field.Sample(6, 2); + Assert.Approximately(1.5, u, 1e-6); + Assert.Approximately(1, v, 1e-6); + Assert.True(float.IsNaN(field.SampleError(0, 0))); + + field.Fill(3, 4); + var histogram = field.MagnitudeHistogram(10, 10); + Assert.Equal(6, histogram[5]); + Assert.Equal(6, field.MagnitudeHistogram(4, 2)[3]); + var stats = field.Statistics(); + Assert.Approximately(5, stats.MeanMagnitude, 1e-6); + Assert.Approximately(5, stats.P95Magnitude, 1e-6); + Assert.Approximately(3, stats.MeanU, 1e-6); + Assert.Equal(1.0, stats.ReliableFraction); + } + + [Test] + public void PyramidLevelsHalveAndPreserveMean() + { + var texture = SyntheticImages.Crop(SyntheticImages.Noise(Base, 1.5, seed: 8), Base, 0, 0, 101, 75); + using var pyramid = new GrayPyramid(101, 75, GrayPyramid.LevelsFor(101, 75)); + Assert.Equal(3, pyramid.Levels); + pyramid.Build(texture); + Assert.Equal(51, pyramid.LevelWidth(1)); + Assert.Equal(38, pyramid.LevelHeight(1)); + Assert.Equal(26, pyramid.LevelWidth(2)); + var mean0 = pyramid.Level(0).ToArray().Average(); + var mean2 = pyramid.Level(2).ToArray().Average(); + Assert.Approximately(mean0, mean2, 0.02); + + var flat = new float[64 * 64]; + flat.AsSpan().Fill(0.25f); + using var constant = new GrayPyramid(64, 64, 3); + constant.Build(flat); + Assert.True(constant.Level(2).ToArray().All(x => x == 0.25f)); + } + + private static double MeanEndPointError(MotionField field, Func truth) + { + double sum = 0; + var n = 0; + for (var j = 0; j < field.Rows; j++) + { + for (var i = 0; i < field.Columns; i++) + { + double x = i * field.Step, y = j * field.Step; + if (x < Margin || y < Margin || x >= Frame - Margin || y >= Frame - Margin) continue; + var (tu, tv) = truth(x, y); + var k = j * field.Columns + i; + sum += double.Hypot(field.U[k] - tu, field.V[k] - tv); + n++; + } + } + return sum / n; + } + + private static ImageBuffer Crop(ImageBuffer source, int offset) + { + var crop = new ImageBuffer(Frame, Frame); + GeometryStage.Warp(source, crop, Affine2D.Translation(offset, offset)); + return crop; + } +}