feat: 开源四阶判决器算法实现（FrameDedupService / FarnebackFlow）与完整训练流水线

16100c3 verified 27 days ago

17.7 kB

using System.Threading.Tasks;

namespace Easy4K.SFDP;

/// <summary>第四阶：Farnebäck 稠密光流（论文第四阶的终判依据）。
///
/// 实现路线与 OpenCV calcOpticalFlowFarneback 同源，但用「加权矩最小二乘」直接拟合多项式系数：
///   1) 灰度化 + 高斯金字塔（自粗到细，pyr_scale / levels）
///   2) 多项式展开：每像素邻域用二次多项式 f(x) ≈ xᵀAx + bᵀx + c 拟合。
///      邻域取 |u|,|v| ≤ poly_n/2 的方形窗，权重为可分离高斯（σ = poly_sigma）；
///      6 个基函数 φ = [1, u, v, u², uv, v²] 的加权矩用 6 组可分离相关一次算完，
///      系数 c = M⁻¹·T（M = Σ W·φφᵀ 只与窗尺寸有关，全图共用一个逆矩阵）。
///   3) 位移求解：第二帧可视为第一帧平移 d → b₂ = b₁ - 2A d，故 A·d = -½(b₂-b₁)。
///      把 A 与 (b₂-b₁) 在 winsize 窗口内累加后解 2×2（比逐像素解稳，与论文一致）。
///   4) 迭代：每轮把第一帧的展开场按当前光流 warp 后重解，位移累加（iterations 次）。
///   5) 金字塔上采样：粗层位移 × 1/pyr_scale 作为细层初值。
///
/// 输出 MeanFlow / VarFlow（位移幅度的一阶/二阶统计，单位：像素），供论文判决使用。
/// 说明：本文件不依赖任何 UI 类型，便于在控制台里做闭环验证。</summary>
public static class FarnebackFlow
{
    /// <summary>参数默认值对齐 OpenCV：levels=3, pyr_scale=0.5, winsize=15, iterations=3, poly_n=5, poly_sigma=1.2</summary>
    public sealed record Options(
        int Levels = 3,
        double PyrScale = 0.5,
        int WinSize = 15,
        int Iterations = 3,
        int PolyN = 5,
        double PolySigma = 1.2);

    /// <summary>光流场 + 论文所需的两项统计</summary>
    public sealed class Field
    {
        public required float[] Dx { get; init; }
        public required float[] Dy { get; init; }
        public int Width { get; init; }
        public int Height { get; init; }
        /// <summary>MeanFlow = 全图位移幅度的平均（像素）</summary>
        public double MeanFlow { get; init; }
        /// <summary>VarFlow = 位移幅度的方差</summary>
        public double VarFlow { get; init; }
    }

    /// <summary>BGRA8 两帧 → 全分辨率稠密光流（第一帧 → 第二帧）。</summary>
    public static Field Compute(byte[] aBgra, byte[] bBgra, int width, int height, Options opt)
    {
        var g1 = ToGray(aBgra, width, height);
        var g2 = ToGray(bBgra, width, height);

        float[]? flowX = null, flowY = null;
        var fw = 0;
        var fh = 0;

        for (var level = opt.Levels - 1; level >= 0; level--)
        {
            var s = Math.Pow(opt.PyrScale, level);
            var w = Math.Max(16, (int)Math.Round(width * s));
            var h = Math.Max(16, (int)Math.Round(height * s));

            var l1 = Resize(g1, width, height, w, h);
            var l2 = Resize(g2, width, height, w, h);

            // 粗层位移放大到本层作为初值（位移量按尺寸比例放大，坐标按比例采样）
            var fx = new float[w * h];
            var fy = new float[w * h];
            if (flowX is not null && fw > 0)
            {
                var kx = w / (double)fw;
                var ky = h / (double)fh;
                for (var y = 0; y < h; y++)
                    for (var x = 0; x < w; x++)
                    {
                        var i = y * w + x;
                        fx[i] = (float)(SampleBilinear(flowX, fw, fh, x / kx, y / ky) * kx);
                        fy[i] = (float)(SampleBilinear(flowY!, fw, fh, x / kx, y / ky) * ky);
                    }
            }

            var e1 = PolyExp(l1, w, h, opt);
            var e2 = PolyExp(l2, w, h, opt);
            for (var it = 0; it < opt.Iterations; it++)
                UpdateFlow(e1, e2, fx, fy, w, h, opt);

            flowX = fx;
            flowY = fy;
            fw = w;
            fh = h;
        }

        var n = fw * fh;
        double mean = 0;
        for (var i = 0; i < n; i++) mean += Magnitude(flowX![i], flowY![i]);
        mean /= Math.Max(1, n);
        double var = 0;
        for (var i = 0; i < n; i++)
        {
            var d = Magnitude(flowX![i], flowY![i]) - mean;
            var += d * d;
        }
        var /= Math.Max(1, n);

        return new Field { Dx = flowX!, Dy = flowY!, Width = fw, Height = fh, MeanFlow = mean, VarFlow = var };
    }

    private static double Magnitude(double dx, double dy) => Math.Sqrt(dx * dx + dy * dy);

    // ===================== 多项式展开 =====================

    /// <summary>每个像素的二次多项式系数（A 对称，只用 Axx/Axy/Ayy + Bx/By）</summary>
    private sealed class Exp
    {
        public required float[] Axx { get; init; }
        public required float[] Axy { get; init; }
        public required float[] Ayy { get; init; }
        public required float[] Bx { get; init; }
        public required float[] By { get; init; }
        public required float[] C { get; init; }
        public int Width { get; init; }
        public int Height { get; init; }
    }

    /// <summary>加权矩最小二乘拟合二次多项式：T_pq = Σ W·u^p v^q · f，系数 = M⁻¹·T</summary>
    private static Exp PolyExp(float[] src, int w, int h, Options opt)
    {
        var k = Math.Max(1, opt.PolyN / 2);
        var g = Gaussian1D(k, opt.PolySigma);

        // 基函数顺序：1, u, v, u², uv, v²  →  矩 (p,q) 取 (0,0),(1,0),(0,1),(2,0),(1,1),(0,2)
        var m00 = Correlate2(src, w, h, g, g, 0, 0);
        var m10 = Correlate2(src, w, h, g, g, 1, 0);
        var m01 = Correlate2(src, w, h, g, g, 0, 1);
        var m20 = Correlate2(src, w, h, g, g, 2, 0);
        var m11 = Correlate2(src, w, h, g, g, 1, 1);
        var m02 = Correlate2(src, w, h, g, g, 0, 2);

        var inv = MomentInverse(k, g);   // 6×6 逆矩阵（只与窗/权重有关）

        var n = w * h;
        var axx = new float[n];
        var axy = new float[n];
        var ayy = new float[n];
        var bx = new float[n];
        var by = new float[n];
        var c0 = new float[n];

        // 注意：t/r 必须线程私有（放在 Parallel.For 外面会被多线程互相覆盖）
        Parallel.For(0, h,
            () => (new double[6], new double[6]),
            (y, _, scratch) =>
            {
                var (t, r) = scratch;
                for (var x = 0; x < w; x++)
                {
                    var i = y * w + x;
                    t[0] = m00[i]; t[1] = m10[i]; t[2] = m01[i];
                    t[3] = m20[i]; t[4] = m11[i]; t[5] = m02[i];
                    for (var r0 = 0; r0 < 6; r0++)
                    {
                        double acc = 0;
                        for (var c = 0; c < 6; c++) acc += inv[r0, c] * t[c];
                        r[r0] = acc;
                    }
                    c0[i] = (float)r[0];
                    bx[i] = (float)r[1];
                    by[i] = (float)r[2];
                    axx[i] = (float)r[3];
                    axy[i] = (float)r[4];
                    ayy[i] = (float)r[5];
                }
                return scratch;
            },
            _ => { });

        return new Exp { Axx = axx, Axy = axy, Ayy = ayy, Bx = bx, By = by, C = c0, Width = w, Height = h };
    }

    /// <summary>长度 2k+1 的归一化高斯核</summary>
    private static double[] Gaussian1D(int k, double sigma)
    {
        var n = 2 * k + 1;
        var g = new double[n];
        double sum = 0;
        for (var i = 0; i < n; i++)
        {
            var u = i - k;
            g[i] = Math.Exp(-(u * u) / (2 * sigma * sigma));
            sum += g[i];
        }
        for (var i = 0; i < n; i++) g[i] /= sum;
        return g;
    }

    /// <summary>可分离相关：out(x,y) = ΣΣ gx(u)·gy(v)·(u/k)^px·(v/k)^py · src(x+u, y+v)，越界按边界复制。
    /// 坐标按半窗宽 k 归一化（Farnebäck 的做法）：平坦区系数不会退化成极小值，
    /// 后面 2×2 求解的条件数才稳定。</summary>
    private static float[] Correlate2(float[] src, int w, int h, double[] g, double[] gy, int px, int py)
    {
        var k = g.Length / 2;
        var tmp = new float[w * h];
        var kx = new double[g.Length];
        for (var i = 0; i < g.Length; i++) kx[i] = g[i] * Math.Pow((i - k) / (double)k, px);

        Parallel.For(0, h, y =>
        {
            for (var x = 0; x < w; x++)
            {
                double acc = 0;
                for (var u = -k; u <= k; u++)
                {
                    var sx = Math.Clamp(x + u, 0, w - 1);
                    acc += kx[u + k] * src[y * w + sx];
                }
                tmp[y * w + x] = (float)acc;
            }
        });

        var ky = new double[g.Length];
        for (var i = 0; i < g.Length; i++) ky[i] = gy[i] * Math.Pow((i - k) / (double)k, py);

        var dst = new float[w * h];
        Parallel.For(0, h, y =>
        {
            for (var x = 0; x < w; x++)
            {
                double acc = 0;
                for (var v = -k; v <= k; v++)
                {
                    var sy = Math.Clamp(y + v, 0, h - 1);
                    acc += ky[v + k] * tmp[sy * w + x];
                }
                dst[y * w + x] = (float)acc;
            }
        });
        return dst;
    }

    /// <summary>M = Σ W·φφᵀ（φ = [1,ũ,ṽ,ũ²,ũṽ,ṽ²]，ũ = u/k）的逆，用高斯-约当求逆。</summary>
    private static double[,] MomentInverse(int k, double[] g)
    {
        var m = new double[6, 6];
        for (var u = -k; u <= k; u++)
            for (var v = -k; v <= k; v++)
            {
                var weight = g[u + k] * g[v + k];
                var un = u / (double)k;
                var vn = v / (double)k;
                var phi = new double[] { 1, un, vn, un * un, un * vn, vn * vn };
                for (var i = 0; i < 6; i++)
                    for (var j = 0; j < 6; j++)
                        m[i, j] += weight * phi[i] * phi[j];
            }
        return Invert6(m);
    }

    private static double[,] Invert6(double[,] a)
    {
        var n = 6;
        var m = new double[n, 2 * n];
        for (var i = 0; i < n; i++)
        {
            for (var j = 0; j < n; j++) m[i, j] = a[i, j];
            m[i, n + i] = 1;
        }
        for (var col = 0; col < n; col++)
        {
            var pivot = col;
            for (var r = col + 1; r < n; r++)
                if (Math.Abs(m[r, col]) > Math.Abs(m[pivot, col])) pivot = r;
            if (Math.Abs(m[pivot, col]) < 1e-12) continue;
            if (pivot != col)
                for (var j = 0; j < 2 * n; j++) (m[col, j], m[pivot, j]) = (m[pivot, j], m[col, j]);
            var d = m[col, col];
            for (var j = 0; j < 2 * n; j++) m[col, j] /= d;
            for (var r = 0; r < n; r++)
            {
                if (r == col) continue;
                var f = m[r, col];
                if (f == 0) continue;
                for (var j = 0; j < 2 * n; j++) m[r, j] -= f * m[col, j];
            }
        }
        var inv = new double[n, n];
        for (var i = 0; i < n; i++)
            for (var j = 0; j < n; j++) inv[i, j] = m[i, n + j];
        return inv;
    }

    // ===================== 位移求解 =====================

    /// <summary>一轮迭代：warp 第一帧展开场 → 逐像素 A、Δb → 窗口累加 → 解 2×2 → 位移累加</summary>
    private static void UpdateFlow(Exp e1, Exp e2, float[] fx, float[] fy, int w, int h, Options opt)
    {
        var n = w * h;
        var g11 = new double[n];
        var g12 = new double[n];
        var g22 = new double[n];
        var h1 = new double[n];
        var h2 = new double[n];

        Parallel.For(0, h, y =>
        {
            for (var x = 0; x < w; x++)
            {
                var i = y * w + x;
                // 第 2 帧位置 x 的内容来自第 1 帧的 x - d，所以第 1 帧展开场要采样在 (x - fx, y - fy)
                var x1 = x - fx[i];
                var y1 = y - fy[i];

                // 第二帧系数（原地）
                var a2xx = e2.Axx[i]; var a2xy = e2.Axy[i]; var a2yy = e2.Ayy[i];
                var b2x = e2.Bx[i]; var b2y = e2.By[i];

                // 第一帧系数按其位移 warp 过来（双线性采样）
                var a1xx = SampleBilinear(e1.Axx, w, h, x1, y1);
                var a1xy = SampleBilinear(e1.Axy, w, h, x1, y1);
                var a1yy = SampleBilinear(e1.Ayy, w, h, x1, y1);
                var b1x = SampleBilinear(e1.Bx, w, h, x1, y1);
                var b1y = SampleBilinear(e1.By, w, h, x1, y1);

                var axx = 0.5 * (a1xx + a2xx);
                var axy = 0.5 * (a1xy + a2xy);
                var ayy = 0.5 * (a1yy + a2yy);
                var rx = b2x - b1x;
                var ry = b2y - b1y;

                // 加权最小二乘的窗口累加量：Σ(AᵀA) 与 Σ(AᵀΔb)
                g11[i] = axx * axx + axy * axy;
                g12[i] = (axx + ayy) * axy;
                g22[i] = axy * axy + ayy * ayy;
                h1[i] = axx * rx + axy * ry;
                h2[i] = axy * rx + ayy * ry;
            }
        });

        var k = Math.Max(1, opt.PolyN / 2);    // 归一化坐标 → 像素的换算因子
        var win = Math.Max(3, opt.WinSize);
        var s11 = BoxSum(g11, w, h, win);
        var s12 = BoxSum(g12, w, h, win);
        var s22 = BoxSum(g22, w, h, win);
        var t1 = BoxSum(h1, w, h, win);
        var t2 = BoxSum(h2, w, h, win);

        const double incLimit = 16.0;          // 单步位移上限（像素），防病态区数值爆炸
        Parallel.For(0, h, y =>
        {
            for (var x = 0; x < w; x++)
            {
                var i = y * w + x;
                var a11 = s11[i];
                var a12 = s12[i];
                var a22 = s22[i];
                var det = a11 * a22 - a12 * a12;

                double dx = 0, dy = 0;
                // 条件数守卫：纹理太弱（两特征值都小）或退化（一条边方向不可观测）时不动
                if (det > 1e-6 && det > 1e-3 * a11 * a22)
                {
                    // Δb = -2A(d-f)  →  d-f = -½(AᵀA)⁻¹AᵀΔb，再乘 k 换回像素
                    var tx = -0.5 * k * (a22 * t1[i] - a12 * t2[i]) / det;
                    var ty = -0.5 * k * (a11 * t2[i] - a12 * t1[i]) / det;
                    var mag = Math.Sqrt(tx * tx + ty * ty);
                    if (mag > incLimit)
                    {
                        tx *= incLimit / mag;
                        ty *= incLimit / mag;
                    }
                    dx = tx;
                    dy = ty;
                }
                fx[i] += (float)dx;
                fy[i] += (float)dy;
            }
        });
    }

    /// <summary>窗口积分（积分图）</summary>
    private static double[] BoxSum(double[] src, int w, int h, int win)
    {
        var r = win / 2;
        var sat = new double[(w + 1) * (h + 1)];
        for (var y = 0; y < h; y++)
        {
            double rowSum = 0;
            for (var x = 0; x < w; x++)
            {
                rowSum += src[y * w + x];
                sat[(y + 1) * (w + 1) + (x + 1)] = sat[y * (w + 1) + (x + 1)] + rowSum;
            }
        }
        var dst = new double[w * h];
        Parallel.For(0, h, y =>
        {
            var y0 = Math.Clamp(y - r, 0, h - 1);
            var y1 = Math.Clamp(y + r, 0, h - 1);
            for (var x = 0; x < w; x++)
            {
                var x0 = Math.Clamp(x - r, 0, w - 1);
                var x1 = Math.Clamp(x + r, 0, w - 1);
                var a = sat[y0 * (w + 1) + x0];
                var b = sat[y0 * (w + 1) + x1 + 1];
                var c = sat[(y1 + 1) * (w + 1) + x0];
                var d = sat[(y1 + 1) * (w + 1) + x1 + 1];
                dst[y * w + x] = d - b - c + a;
            }
        });
        return dst;
    }

    // ===================== 图像工具 =====================

    private static float[] ToGray(byte[] bgra, int w, int h)
    {
        var n = w * h;
        var g = new float[n];
        for (int i = 0, p = 0; i < n; i++, p += 4)
            g[i] = (bgra[p + 2] * 299 + bgra[p + 1] * 587 + bgra[p] * 114) / 1000f;
        return g;
    }

    /// <summary>双线性缩放（像素中心对齐）</summary>
    private static float[] Resize(float[] src, int sw, int sh, int dw, int dh)
    {
        var dst = new float[dw * dh];
        var kx = sw / (double)dw;
        var ky = sh / (double)dh;
        Parallel.For(0, dh, y =>
        {
            var sy = (y + 0.5) * ky - 0.5;
            for (var x = 0; x < dw; x++)
                dst[y * dw + x] = (float)SampleBilinear(src, sw, sh, (x + 0.5) * kx - 0.5, sy);
        });
        return dst;
    }

    private static double SampleBilinear(float[] src, int w, int h, double x, double y)
    {
        if (w <= 0 || h <= 0) return 0;
        var cx = Math.Clamp(x, 0, w - 1);
        var cy = Math.Clamp(y, 0, h - 1);
        var x0 = (int)Math.Floor(cx);
        var y0 = (int)Math.Floor(cy);
        var x1 = Math.Min(x0 + 1, w - 1);
        var y1 = Math.Min(y0 + 1, h - 1);
        var tx = cx - x0;
        var ty = cy - y0;
        var v00 = src[y0 * w + x0];
        var v10 = src[y0 * w + x1];
        var v01 = src[y1 * w + x0];
        var v11 = src[y1 * w + x1];
        return (v00 * (1 - tx) + v10 * tx) * (1 - ty) + (v01 * (1 - tx) + v11 * tx) * ty;
    }
}

	using System.Threading.Tasks;

	namespace Easy4K.SFDP;

	/// <summary>第四阶：Farnebäck 稠密光流（论文第四阶的终判依据）。
	///
	/// 实现路线与 OpenCV calcOpticalFlowFarneback 同源，但用「加权矩最小二乘」直接拟合多项式系数：
	/// 1) 灰度化 + 高斯金字塔（自粗到细，pyr_scale / levels）
	/// 2) 多项式展开：每像素邻域用二次多项式 f(x) ≈ xᵀAx + bᵀx + c 拟合。
	/// 邻域取 \|u\|,\|v\| ≤ poly_n/2 的方形窗，权重为可分离高斯（σ = poly_sigma）；
	/// 6 个基函数 φ = [1, u, v, u², uv, v²] 的加权矩用 6 组可分离相关一次算完，
	/// 系数 c = M⁻¹·T（M = Σ W·φφᵀ 只与窗尺寸有关，全图共用一个逆矩阵）。
	/// 3) 位移求解：第二帧可视为第一帧平移 d → b₂ = b₁ - 2A d，故 A·d = -½(b₂-b₁)。
	/// 把 A 与 (b₂-b₁) 在 winsize 窗口内累加后解 2×2（比逐像素解稳，与论文一致）。
	/// 4) 迭代：每轮把第一帧的展开场按当前光流 warp 后重解，位移累加（iterations 次）。
	/// 5) 金字塔上采样：粗层位移 × 1/pyr_scale 作为细层初值。
	///
	/// 输出 MeanFlow / VarFlow（位移幅度的一阶/二阶统计，单位：像素），供论文判决使用。
	/// 说明：本文件不依赖任何 UI 类型，便于在控制台里做闭环验证。</summary>
	public static class FarnebackFlow
	{
	/// <summary>参数默认值对齐 OpenCV：levels=3, pyr_scale=0.5, winsize=15, iterations=3, poly_n=5, poly_sigma=1.2</summary>
	public sealed record Options(
	int Levels = 3,
	double PyrScale = 0.5,
	int WinSize = 15,
	int Iterations = 3,
	int PolyN = 5,
	double PolySigma = 1.2);

	/// <summary>光流场 + 论文所需的两项统计</summary>
	public sealed class Field
	{
	public required float[] Dx { get; init; }
	public required float[] Dy { get; init; }
	public int Width { get; init; }
	public int Height { get; init; }
	/// <summary>MeanFlow = 全图位移幅度的平均（像素）</summary>
	public double MeanFlow { get; init; }
	/// <summary>VarFlow = 位移幅度的方差</summary>
	public double VarFlow { get; init; }
	}

	/// <summary>BGRA8 两帧 → 全分辨率稠密光流（第一帧 → 第二帧）。</summary>
	public static Field Compute(byte[] aBgra, byte[] bBgra, int width, int height, Options opt)
	{
	var g1 = ToGray(aBgra, width, height);
	var g2 = ToGray(bBgra, width, height);

	float[]? flowX = null, flowY = null;
	var fw = 0;
	var fh = 0;

	for (var level = opt.Levels - 1; level >= 0; level--)
	{
	var s = Math.Pow(opt.PyrScale, level);
	var w = Math.Max(16, (int)Math.Round(width * s));
	var h = Math.Max(16, (int)Math.Round(height * s));

	var l1 = Resize(g1, width, height, w, h);
	var l2 = Resize(g2, width, height, w, h);

	// 粗层位移放大到本层作为初值（位移量按尺寸比例放大，坐标按比例采样）
	var fx = new float[w * h];
	var fy = new float[w * h];
	if (flowX is not null && fw > 0)
	{
	var kx = w / (double)fw;
	var ky = h / (double)fh;
	for (var y = 0; y < h; y++)
	for (var x = 0; x < w; x++)
	{
	var i = y * w + x;
	fx[i] = (float)(SampleBilinear(flowX, fw, fh, x / kx, y / ky) * kx);
	fy[i] = (float)(SampleBilinear(flowY!, fw, fh, x / kx, y / ky) * ky);
	}
	}

	var e1 = PolyExp(l1, w, h, opt);
	var e2 = PolyExp(l2, w, h, opt);
	for (var it = 0; it < opt.Iterations; it++)
	UpdateFlow(e1, e2, fx, fy, w, h, opt);

	flowX = fx;
	flowY = fy;
	fw = w;
	fh = h;
	}

	var n = fw * fh;
	double mean = 0;
	for (var i = 0; i < n; i++) mean += Magnitude(flowX![i], flowY![i]);
	mean /= Math.Max(1, n);
	double var = 0;
	for (var i = 0; i < n; i++)
	{
	var d = Magnitude(flowX![i], flowY![i]) - mean;
	var += d * d;
	}
	var /= Math.Max(1, n);

	return new Field { Dx = flowX!, Dy = flowY!, Width = fw, Height = fh, MeanFlow = mean, VarFlow = var };
	}

	private static double Magnitude(double dx, double dy) => Math.Sqrt(dx * dx + dy * dy);

	// ===================== 多项式展开 =====================

	/// <summary>每个像素的二次多项式系数（A 对称，只用 Axx/Axy/Ayy + Bx/By）</summary>
	private sealed class Exp
	{
	public required float[] Axx { get; init; }
	public required float[] Axy { get; init; }
	public required float[] Ayy { get; init; }
	public required float[] Bx { get; init; }
	public required float[] By { get; init; }
	public required float[] C { get; init; }
	public int Width { get; init; }
	public int Height { get; init; }
	}

	/// <summary>加权矩最小二乘拟合二次多项式：T_pq = Σ W·u^p v^q · f，系数 = M⁻¹·T</summary>
	private static Exp PolyExp(float[] src, int w, int h, Options opt)
	{
	var k = Math.Max(1, opt.PolyN / 2);
	var g = Gaussian1D(k, opt.PolySigma);

	// 基函数顺序：1, u, v, u², uv, v² → 矩 (p,q) 取 (0,0),(1,0),(0,1),(2,0),(1,1),(0,2)
	var m00 = Correlate2(src, w, h, g, g, 0, 0);
	var m10 = Correlate2(src, w, h, g, g, 1, 0);
	var m01 = Correlate2(src, w, h, g, g, 0, 1);
	var m20 = Correlate2(src, w, h, g, g, 2, 0);
	var m11 = Correlate2(src, w, h, g, g, 1, 1);
	var m02 = Correlate2(src, w, h, g, g, 0, 2);

	var inv = MomentInverse(k, g); // 6×6 逆矩阵（只与窗/权重有关）

	var n = w * h;
	var axx = new float[n];
	var axy = new float[n];
	var ayy = new float[n];
	var bx = new float[n];
	var by = new float[n];
	var c0 = new float[n];

	// 注意：t/r 必须线程私有（放在 Parallel.For 外面会被多线程互相覆盖）
	Parallel.For(0, h,
	() => (new double[6], new double[6]),
	(y, _, scratch) =>
	{
	var (t, r) = scratch;
	for (var x = 0; x < w; x++)
	{
	var i = y * w + x;
	t[0] = m00[i]; t[1] = m10[i]; t[2] = m01[i];
	t[3] = m20[i]; t[4] = m11[i]; t[5] = m02[i];
	for (var r0 = 0; r0 < 6; r0++)
	{
	double acc = 0;
	for (var c = 0; c < 6; c++) acc += inv[r0, c] * t[c];
	r[r0] = acc;
	}
	c0[i] = (float)r[0];
	bx[i] = (float)r[1];
	by[i] = (float)r[2];
	axx[i] = (float)r[3];
	axy[i] = (float)r[4];
	ayy[i] = (float)r[5];
	}
	return scratch;
	},
	_ => { });

	return new Exp { Axx = axx, Axy = axy, Ayy = ayy, Bx = bx, By = by, C = c0, Width = w, Height = h };
	}

	/// <summary>长度 2k+1 的归一化高斯核</summary>
	private static double[] Gaussian1D(int k, double sigma)
	{
	var n = 2 * k + 1;
	var g = new double[n];
	double sum = 0;
	for (var i = 0; i < n; i++)
	{
	var u = i - k;
	g[i] = Math.Exp(-(u * u) / (2 * sigma * sigma));
	sum += g[i];
	}
	for (var i = 0; i < n; i++) g[i] /= sum;
	return g;
	}

	/// <summary>可分离相关：out(x,y) = ΣΣ gx(u)·gy(v)·(u/k)^px·(v/k)^py · src(x+u, y+v)，越界按边界复制。
	/// 坐标按半窗宽 k 归一化（Farnebäck 的做法）：平坦区系数不会退化成极小值，
	/// 后面 2×2 求解的条件数才稳定。</summary>
	private static float[] Correlate2(float[] src, int w, int h, double[] g, double[] gy, int px, int py)
	{
	var k = g.Length / 2;
	var tmp = new float[w * h];
	var kx = new double[g.Length];
	for (var i = 0; i < g.Length; i++) kx[i] = g[i] * Math.Pow((i - k) / (double)k, px);

	Parallel.For(0, h, y =>
	{
	for (var x = 0; x < w; x++)
	{
	double acc = 0;
	for (var u = -k; u <= k; u++)
	{
	var sx = Math.Clamp(x + u, 0, w - 1);
	acc += kx[u + k] * src[y * w + sx];
	}
	tmp[y * w + x] = (float)acc;
	}
	});

	var ky = new double[g.Length];
	for (var i = 0; i < g.Length; i++) ky[i] = gy[i] * Math.Pow((i - k) / (double)k, py);

	var dst = new float[w * h];
	Parallel.For(0, h, y =>
	{
	for (var x = 0; x < w; x++)
	{
	double acc = 0;
	for (var v = -k; v <= k; v++)
	{
	var sy = Math.Clamp(y + v, 0, h - 1);
	acc += ky[v + k] * tmp[sy * w + x];
	}
	dst[y * w + x] = (float)acc;
	}
	});
	return dst;
	}

	/// <summary>M = Σ W·φφᵀ（φ = [1,ũ,ṽ,ũ²,ũṽ,ṽ²]，ũ = u/k）的逆，用高斯-约当求逆。</summary>
	private static double[,] MomentInverse(int k, double[] g)
	{
	var m = new double[6, 6];
	for (var u = -k; u <= k; u++)
	for (var v = -k; v <= k; v++)
	{
	var weight = g[u + k] * g[v + k];
	var un = u / (double)k;
	var vn = v / (double)k;
	var phi = new double[] { 1, un, vn, un * un, un * vn, vn * vn };
	for (var i = 0; i < 6; i++)
	for (var j = 0; j < 6; j++)
	m[i, j] += weight * phi[i] * phi[j];
	}
	return Invert6(m);
	}

	private static double[,] Invert6(double[,] a)
	{
	var n = 6;
	var m = new double[n, 2 * n];
	for (var i = 0; i < n; i++)
	{
	for (var j = 0; j < n; j++) m[i, j] = a[i, j];
	m[i, n + i] = 1;
	}
	for (var col = 0; col < n; col++)
	{
	var pivot = col;
	for (var r = col + 1; r < n; r++)
	if (Math.Abs(m[r, col]) > Math.Abs(m[pivot, col])) pivot = r;
	if (Math.Abs(m[pivot, col]) < 1e-12) continue;
	if (pivot != col)
	for (var j = 0; j < 2 * n; j++) (m[col, j], m[pivot, j]) = (m[pivot, j], m[col, j]);
	var d = m[col, col];
	for (var j = 0; j < 2 * n; j++) m[col, j] /= d;
	for (var r = 0; r < n; r++)
	{
	if (r == col) continue;
	var f = m[r, col];
	if (f == 0) continue;
	for (var j = 0; j < 2 * n; j++) m[r, j] -= f * m[col, j];
	}
	}
	var inv = new double[n, n];
	for (var i = 0; i < n; i++)
	for (var j = 0; j < n; j++) inv[i, j] = m[i, n + j];
	return inv;
	}

	// ===================== 位移求解 =====================

	/// <summary>一轮迭代：warp 第一帧展开场 → 逐像素 A、Δb → 窗口累加 → 解 2×2 → 位移累加</summary>
	private static void UpdateFlow(Exp e1, Exp e2, float[] fx, float[] fy, int w, int h, Options opt)
	{
	var n = w * h;
	var g11 = new double[n];
	var g12 = new double[n];
	var g22 = new double[n];
	var h1 = new double[n];
	var h2 = new double[n];

	Parallel.For(0, h, y =>
	{
	for (var x = 0; x < w; x++)
	{
	var i = y * w + x;
	// 第 2 帧位置 x 的内容来自第 1 帧的 x - d，所以第 1 帧展开场要采样在 (x - fx, y - fy)
	var x1 = x - fx[i];
	var y1 = y - fy[i];

	// 第二帧系数（原地）
	var a2xx = e2.Axx[i]; var a2xy = e2.Axy[i]; var a2yy = e2.Ayy[i];
	var b2x = e2.Bx[i]; var b2y = e2.By[i];

	// 第一帧系数按其位移 warp 过来（双线性采样）
	var a1xx = SampleBilinear(e1.Axx, w, h, x1, y1);
	var a1xy = SampleBilinear(e1.Axy, w, h, x1, y1);
	var a1yy = SampleBilinear(e1.Ayy, w, h, x1, y1);
	var b1x = SampleBilinear(e1.Bx, w, h, x1, y1);
	var b1y = SampleBilinear(e1.By, w, h, x1, y1);

	var axx = 0.5 * (a1xx + a2xx);
	var axy = 0.5 * (a1xy + a2xy);
	var ayy = 0.5 * (a1yy + a2yy);
	var rx = b2x - b1x;
	var ry = b2y - b1y;

	// 加权最小二乘的窗口累加量：Σ(AᵀA) 与 Σ(AᵀΔb)
	g11[i] = axx * axx + axy * axy;
	g12[i] = (axx + ayy) * axy;
	g22[i] = axy * axy + ayy * ayy;
	h1[i] = axx * rx + axy * ry;
	h2[i] = axy * rx + ayy * ry;
	}
	});

	var k = Math.Max(1, opt.PolyN / 2); // 归一化坐标 → 像素的换算因子
	var win = Math.Max(3, opt.WinSize);
	var s11 = BoxSum(g11, w, h, win);
	var s12 = BoxSum(g12, w, h, win);
	var s22 = BoxSum(g22, w, h, win);
	var t1 = BoxSum(h1, w, h, win);
	var t2 = BoxSum(h2, w, h, win);

	const double incLimit = 16.0; // 单步位移上限（像素），防病态区数值爆炸
	Parallel.For(0, h, y =>
	{
	for (var x = 0; x < w; x++)
	{
	var i = y * w + x;
	var a11 = s11[i];
	var a12 = s12[i];
	var a22 = s22[i];
	var det = a11 * a22 - a12 * a12;

	double dx = 0, dy = 0;
	// 条件数守卫：纹理太弱（两特征值都小）或退化（一条边方向不可观测）时不动
	if (det > 1e-6 && det > 1e-3 * a11 * a22)
	{
	// Δb = -2A(d-f) → d-f = -½(AᵀA)⁻¹AᵀΔb，再乘 k 换回像素
	var tx = -0.5 * k * (a22 * t1[i] - a12 * t2[i]) / det;
	var ty = -0.5 * k * (a11 * t2[i] - a12 * t1[i]) / det;
	var mag = Math.Sqrt(tx * tx + ty * ty);
	if (mag > incLimit)
	{
	tx *= incLimit / mag;
	ty *= incLimit / mag;
	}
	dx = tx;
	dy = ty;
	}
	fx[i] += (float)dx;
	fy[i] += (float)dy;
	}
	});
	}

	/// <summary>窗口积分（积分图）</summary>
	private static double[] BoxSum(double[] src, int w, int h, int win)
	{
	var r = win / 2;
	var sat = new double[(w + 1) * (h + 1)];
	for (var y = 0; y < h; y++)
	{
	double rowSum = 0;
	for (var x = 0; x < w; x++)
	{
	rowSum += src[y * w + x];
	sat[(y + 1) * (w + 1) + (x + 1)] = sat[y * (w + 1) + (x + 1)] + rowSum;
	}
	}
	var dst = new double[w * h];
	Parallel.For(0, h, y =>
	{
	var y0 = Math.Clamp(y - r, 0, h - 1);
	var y1 = Math.Clamp(y + r, 0, h - 1);
	for (var x = 0; x < w; x++)
	{
	var x0 = Math.Clamp(x - r, 0, w - 1);
	var x1 = Math.Clamp(x + r, 0, w - 1);
	var a = sat[y0 * (w + 1) + x0];
	var b = sat[y0 * (w + 1) + x1 + 1];
	var c = sat[(y1 + 1) * (w + 1) + x0];
	var d = sat[(y1 + 1) * (w + 1) + x1 + 1];
	dst[y * w + x] = d - b - c + a;
	}
	});
	return dst;
	}

	// ===================== 图像工具 =====================

	private static float[] ToGray(byte[] bgra, int w, int h)
	{
	var n = w * h;
	var g = new float[n];
	for (int i = 0, p = 0; i < n; i++, p += 4)
	g[i] = (bgra[p + 2] * 299 + bgra[p + 1] * 587 + bgra[p] * 114) / 1000f;
	return g;
	}

	/// <summary>双线性缩放（像素中心对齐）</summary>
	private static float[] Resize(float[] src, int sw, int sh, int dw, int dh)
	{
	var dst = new float[dw * dh];
	var kx = sw / (double)dw;
	var ky = sh / (double)dh;
	Parallel.For(0, dh, y =>
	{
	var sy = (y + 0.5) * ky - 0.5;
	for (var x = 0; x < dw; x++)
	dst[y * dw + x] = (float)SampleBilinear(src, sw, sh, (x + 0.5) * kx - 0.5, sy);
	});
	return dst;
	}

	private static double SampleBilinear(float[] src, int w, int h, double x, double y)
	{
	if (w <= 0 \|\| h <= 0) return 0;
	var cx = Math.Clamp(x, 0, w - 1);
	var cy = Math.Clamp(y, 0, h - 1);
	var x0 = (int)Math.Floor(cx);
	var y0 = (int)Math.Floor(cy);
	var x1 = Math.Min(x0 + 1, w - 1);
	var y1 = Math.Min(y0 + 1, h - 1);
	var tx = cx - x0;
	var ty = cy - y0;
	var v00 = src[y0 * w + x0];
	var v10 = src[y0 * w + x1];
	var v01 = src[y1 * w + x0];
	var v11 = src[y1 * w + x1];
	return (v00 * (1 - tx) + v10 * tx) * (1 - ty) + (v01 * (1 - tx) + v11 * tx) * ty;
	}
	}