diff --git a/src/Titano.Core/Motion/PathSmoother.cs b/src/Titano.Core/Motion/PathSmoother.cs new file mode 100644 index 00000000..7c54fcdb --- /dev/null +++ b/src/Titano.Core/Motion/PathSmoother.cs @@ -0,0 +1,40 @@ +namespace Titano.Core.Motion; + +/// Smoothing of per-frame series (camera paths). +internal static class PathSmoother +{ + /// + /// Gaussian-weighted local linear regression (degree-1 LOESS with a Gaussian kernel truncated at 3σ): + /// each sample is replaced by the value at its own position of a line fitted to its neighbourhood. + /// Linear trends (pans) pass unchanged and the ends are not pulled toward the interior, unlike a plain + /// Gaussian average. σ ≤ 0 returns a copy. + /// + public static double[] LocalLinear(ReadOnlySpan values, double sigma) + { + var n = values.Length; + var result = values.ToArray(); + if (!(sigma > 0) || n < 2) return result; + var radius = (int)double.Ceiling(3 * sigma); + Span kernel = radius <= 1024 ? stackalloc double[radius + 1] : new double[radius + 1]; + for (var d = 0; d <= radius; d++) kernel[d] = double.Exp(-0.5 * d * d / (sigma * sigma)); + for (var k = 0; k < n; k++) + { + double s0 = 0, s1 = 0, s2 = 0, t0 = 0, t1 = 0; + var lo = int.Max(0, k - radius); + var hi = int.Min(n - 1, k + radius); + for (var j = lo; j <= hi; j++) + { + var d = j - k; + var w = kernel[int.Abs(d)]; + s0 += w; + s1 += w * d; + s2 += w * d * d; + t0 += w * values[j]; + t1 += w * d * values[j]; + } + var det = s0 * s2 - s1 * s1; + result[k] = det > 1e-12 * s0 * s0 ? (s2 * t0 - s1 * t1) / det : t0 / s0; + } + return result; + } +} diff --git a/src/Titano.Core/Motion/Stabilizer.cs b/src/Titano.Core/Motion/Stabilizer.cs new file mode 100644 index 00000000..7bb44d60 --- /dev/null +++ b/src/Titano.Core/Motion/Stabilizer.cs @@ -0,0 +1,409 @@ +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) and |ln scale| of a hypothesis. + public double MaxRotation { get; init; } = 0.05; + + 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, 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 holds two +/// luminance planes regardless of sequence length. +/// +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 _correlator; + 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); + _correlator = new PhaseCorrelator(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]; + _smooth = new float[PatchSize * PatchSize]; + _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; + for (var i = 0; i < 9; i++) _currentEnergy[i] = TextureEnergy(current, _patches[i].X, _patches[i].Y); + + FrameMotion motion; + if (_motions.Count == 0) + { + motion = new FrameMotion(Affine2D.Identity, 0, 0); + } + else + { + var count = 0; + var half = (PatchSize - 1) / 2.0; + for (var i = 0; i < 9; i++) + { + if (double.Min(_previousEnergy[i], _currentEnergy[i]) < Options.TextureThreshold) continue; + var (x0, y0) = _patches[i]; + var r = _correlator.Correlate(_previous, current, Width, x0, y0); + if (r.Confidence < Options.MinimumConfidence) continue; + _matches[count++] = new Match(x0 + half, y0 + half, x0 + half + r.Dx, y0 + half + r.Dy, r.Confidence); + } + motion = 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) + { + 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); +} diff --git a/tests/Titano.Core.Tests/Motion/StabilizerTests.cs b/tests/Titano.Core.Tests/Motion/StabilizerTests.cs new file mode 100644 index 00000000..aedd1119 --- /dev/null +++ b/tests/Titano.Core.Tests/Motion/StabilizerTests.cs @@ -0,0 +1,171 @@ +using Titano.Core.Imaging; +using Titano.Core.Imaging.Geometry; +using Titano.Core.Motion; +using Titano.Testing; + +namespace Titano.Core.Tests.Motion; + +public sealed class StabilizerTests +{ + private const int Width = 256; + private const int Height = 192; + private const int Frames = 48; + + private readonly TestContext _context; + + public StabilizerTests(TestContext context) => _context = context; + + [Test] + public void TexturelessFramesReportNoShakeAndNoCorrection() + { + var random = new Random(17); + using var stabilizer = new Stabilizer(128, 128); + using var frame = new ImageBuffer(128, 128); + for (var k = 0; k < 20; k++) + { + var data = frame.Data; + for (var i = 0; i < data.Length; i++) data[i] = 0.3f + 0.004f * (float)Gaussian(random); + Assert.False(stabilizer.AddFrame(frame).Estimated); + } + var result = stabilizer.Solve(); + _context.Log($"textureless shake {result.ShakeRms:F4} px, crop scale {result.CropScale:F4}"); + Assert.LessOrEqual(result.ShakeRms, 0.1); + Assert.False(result.Recommended); + Assert.Equal(1.0, result.CropScale); + Assert.True(result.Corrections.All(c => c == Affine2D.Identity)); + } + + [Test] + public void JitterIsReducedByAtLeastEightyPercent() + { + var random = new Random(23); + var cameras = new Affine2D[Frames]; + for (var k = 0; k < Frames; k++) cameras[k] = Camera(2.0 * Gaussian(random), 2.0 * Gaussian(random), 0.3 * Gaussian(random), pan: 0, k); + var result = Stabilize(cameras, out var motionError); + var before = Jitter(cameras, null); + var after = Jitter(cameras, result.Corrections); + var reduction = 1 - after / before; + _context.Log($"frame-to-frame jitter {before:F3} → {after:F3} px ({reduction:P1}); motion error {motionError:F4} px; shake {result.ShakeRms:F3} px; crop cost {result.CropCost:P1}"); + Assert.GreaterOrEqual(reduction, 0.8); + Assert.Less(motionError, 0.1); + Assert.True(result.Recommended); + Assert.Greater(result.ShakeRms, 1.0); + Assert.InRange(result.CropCost, 0.01, 0.3); + } + + [Test] + public void IntentionalPanSurvivesStabilisation() + { + const double pan = 1.5; + var random = new Random(29); + var cameras = new Affine2D[Frames]; + for (var k = 0; k < Frames; k++) cameras[k] = Camera(1.5 * Gaussian(random), 1.5 * Gaussian(random), 0.2 * Gaussian(random), pan, k); + var result = Stabilize(cameras, out _); + + // The stabilised view still follows the pan: content drifts left by `pan` px per frame. + var (bx, by) = cameras[0].Apply(Width / 2.0, Height / 2.0); + (double X, double Y) Track(int k) + { + var (x, y) = cameras[k].Invert().Apply(bx, by); + return result.Corrections[k].Apply(x, y); + } + var (sx, sy) = Track(0); + var (ex, ey) = Track(Frames - 1); + var velocity = (ex - sx) / (Frames - 1); + _context.Log($"stabilised velocity {velocity:F3} px/frame (pan {-pan}), vertical drift {ey - sy:F3} px"); + Assert.Approximately(-pan, velocity, 0.1); + var reduction = 1 - Jitter(cameras, result.Corrections, removeMean: true) / Jitter(cameras, null, removeMean: true); + Assert.GreaterOrEqual(reduction, 0.8); + } + + [Test] + public void SmootherKeepsLinesAndEnds() + { + var line = Enumerable.Range(0, 30).Select(i => 3 + 0.5 * i).ToArray(); + var smoothed = PathSmoother.LocalLinear(line, 5); + for (var i = 0; i < line.Length; i++) Assert.Approximately(line[i], smoothed[i], 1e-9); + // Frame-rate jitter (alternating ±1) is removed in the interior and strongly attenuated at the ends. + var noisy = Enumerable.Range(0, 200).Select(i => i % 2 == 0 ? 1.0 : -1.0).ToArray(); + var flat = PathSmoother.LocalLinear(noisy, 8); + var interior = flat.Skip(24).SkipLast(24).Max(double.Abs); + var ends = flat.Max(double.Abs); + _context.Log($"interior {interior:E2}, ends {ends:E2}"); + Assert.Less(interior, 0.01); + Assert.Less(ends, 0.2); + } + + /// Frame pixel → base-texture coordinate: jitter translation and rotation about the frame centre, plus a pan. + private static Affine2D Camera(double tx, double ty, double degrees, double pan, int k) => + Affine2D.Translation(100 + pan * k, 150) + * Affine2D.About(Affine2D.Rotation(degrees * double.Pi / 180), (Width - 1) / 2.0, (Height - 1) / 2.0) + * Affine2D.Translation(tx, ty); + + private static StabilizationResult Stabilize(Affine2D[] cameras, out double motionError) + { + const int Base = 512; + using var texture = SyntheticImages.ToRgb(SyntheticImages.Crop(SyntheticImages.Natural(Base, seed: 41), Base, 0, 0, Base, Base), Base, Base); + using var frame = new ImageBuffer(Width, Height); + using var stabilizer = new Stabilizer(Width, Height); + double error = 0; + var samples = 0; + for (var k = 0; k < cameras.Length; k++) + { + GeometryStage.Warp(texture, frame, cameras[k]); + var motion = stabilizer.AddFrame(frame); + if (k == 0) continue; + // True inter-frame map: previous frame → base → current frame. + var truth = cameras[k].Invert() * cameras[k - 1]; + foreach (var (x, y) in GridPoints()) + { + var (tx, ty) = truth.Apply(x, y); + var (mx, my) = motion.Transform.Apply(x, y); + error += double.Hypot(tx - mx, ty - my); + samples++; + } + } + motionError = error / samples; + return stabilizer.Solve(); + } + + /// RMS frame-to-frame displacement of fixed content points in the (corrected) frames. + private static double Jitter(Affine2D[] cameras, IReadOnlyList? corrections, bool removeMean = false) + { + var points = GridPoints().Select(p => cameras[0].Apply(p.X, p.Y)).ToArray(); + var steps = new List<(double X, double Y)>(); + for (var k = 1; k < cameras.Length; k++) + { + foreach (var (bx, by) in points) + { + var (x0, y0) = cameras[k - 1].Invert().Apply(bx, by); + var (x1, y1) = cameras[k].Invert().Apply(bx, by); + if (corrections is not null) + { + (x0, y0) = corrections[k - 1].Apply(x0, y0); + (x1, y1) = corrections[k].Apply(x1, y1); + } + steps.Add((x1 - x0, y1 - y0)); + } + } + double mx = 0, my = 0; + if (removeMean) + { + mx = steps.Average(s => s.X); + my = steps.Average(s => s.Y); + } + return double.Sqrt(steps.Average(s => (s.X - mx) * (s.X - mx) + (s.Y - my) * (s.Y - my))); + } + + private static IEnumerable<(double X, double Y)> GridPoints() + { + for (var j = 1; j <= 3; j++) + for (var i = 1; i <= 3; i++) + yield return (Width * i / 4.0, Height * j / 4.0); + } + + private static double Gaussian(Random random) + { + var u1 = 1.0 - random.NextDouble(); + var u2 = random.NextDouble(); + return double.Sqrt(-2 * double.Log(u1)) * double.Cos(2 * double.Pi * u2); + } +}