aivfo-tl-3.0/ivf_tl_operate_2.0/control/IvfTl.AutoFocus/Imaging/Sharpness.cs

using System;

namespace IvfTl.AutoFocus.Imaging
{
    /// <summary>
    /// 清晰度评价（纯 C#，无 OpenCV 依赖）。
    /// 按总方案：梯度类指标（Tenengrad 主 / Laplacian 备），锁 ROI、缩图、除均值归一化。
    /// 别用灰度方差/对比度（会被"暗+高反差"骗）。
    ///
    /// 输入统一为相机的 24bpp BGR 字节数组（未翻正也可，清晰度与上下翻转无关）。
    /// 【移植说明 M2-01】逐字搬自 autofocustool/Imaging/Sharpness.cs，仅改命名空间。
    /// ÷mean 一次的真机修复(行尾)逐字保留(03 §1)。
    /// </summary>
    public static class Sharpness
    {
        /// <summary>清晰度算法种类</summary>
        public enum Metric { Tenengrad, Laplacian }

        /// <summary>
        /// 计算清晰度分数（越大越清晰，已除均值归一化，可跨帧比较）。
        /// </summary>
        /// <param name="bgr24">相机原始 24bpp BGR 字节</param>
        /// <param name="width">图宽</param>
        /// <param name="height">图高</param>
        /// <param name="roi">ROI（像素坐标，null=全图）。只在 ROI 内算分，排除背景反光。</param>
        /// <param name="metric">指标类型</param>
        public static double Compute(byte[] bgr24, int width, int height,
            System.Drawing.Rectangle? roi = null, Metric metric = Metric.Tenengrad)
        {
            // 1) 转灰度（只取 ROI 区域，行优先）
            var (gray, gw, gh) = ToGrayRoi(bgr24, width, height, roi);
            if (gw < 3 || gh < 3) return 0;

            // 2) 梯度算分
            return metric == Metric.Laplacian
                ? LaplacianScore(gray, gw, gh)
                : TenengradScore(gray, gw, gh);
        }

        /// <summary>BGR → 8bit 灰度，裁出 ROI。返回灰度数组+宽高。</summary>
        private static (byte[] gray, int w, int h) ToGrayRoi(
            byte[] bgr24, int width, int height, System.Drawing.Rectangle? roi)
        {
            int x0 = 0, y0 = 0, w = width, h = height;
            if (roi.HasValue)
            {
                var r = roi.Value;
                x0 = Math.Max(0, r.X);
                y0 = Math.Max(0, r.Y);
                w = Math.Min(r.Width, width - x0);
                h = Math.Min(r.Height, height - y0);
                if (w <= 0 || h <= 0) { x0 = 0; y0 = 0; w = width; h = height; }
            }

            var gray = new byte[w * h];
            int stride = width * 3;
            for (int y = 0; y < h; y++)
            {
                int srcRow = (y0 + y) * stride;
                int dstRow = y * w;
                for (int x = 0; x < w; x++)
                {
                    int p = srcRow + (x0 + x) * 3;
                    // BGR: 0.114B + 0.587G + 0.299R
                    byte b = bgr24[p], g = bgr24[p + 1], rr = bgr24[p + 2];
                    gray[dstRow + x] = (byte)((b * 29 + g * 150 + rr * 77) >> 8);
                }
            }
            return (gray, w, h);
        }

        /// <summary>
        /// Tenengrad：Sobel 梯度幅值平方和，除以像素数与均值灰度（归一化）。
        /// </summary>
        private static double TenengradScore(byte[] g, int w, int h)
        {
            double sumSq = 0;
            long sumGray = 0;
            for (int i = 0; i < g.Length; i++) sumGray += g[i];
            double mean = (double)sumGray / g.Length;
            if (mean < 1e-6) mean = 1e-6;

            for (int y = 1; y < h - 1; y++)
            {
                int row = y * w;
                for (int x = 1; x < w - 1; x++)
                {
                    int i = row + x;
                    // Sobel Gx
                    int gx = (g[i - w + 1] + 2 * g[i + 1] + g[i + w + 1])
                           - (g[i - w - 1] + 2 * g[i - 1] + g[i + w - 1]);
                    // Sobel Gy
                    int gy = (g[i + w - 1] + 2 * g[i + w] + g[i + w + 1])
                           - (g[i - w - 1] + 2 * g[i - w] + g[i - w + 1]);
                    sumSq += (double)gx * gx + (double)gy * gy;
                }
            }
            int n = (w - 2) * (h - 2);
            if (n <= 0) return 0;
            // 除以像素数（消除 ROI 尺寸影响）+ 除以均值（归一化亮度，但只除一次方）。
            // 【真机修正 2026-06-16】原为除以 mean*mean（均值平方）。实测 4 号舱 well1 的 Z 清晰度曲线
            // （TestData/zcurve_w1_*）显示：随 Z 增大画面亮度从 ~53 单调升到 ~185（光学效应），
            // mean² 会把高 Z 的清晰帧分数严重压低，导致算法把最暗最糊的低 Z 层（z=20000）误判为
            // 最清晰，真实焦面（z≈92000，原始梯度峰）反而落选 → 对焦落到错误位置。
            // 改为除以 mean（一次方）：既保留对亮度的鲁棒性，又不过度压制高亮清晰帧，
            // 反算验证峰正确落在 92000。
            return sumSq / n / mean;
        }

        /// <summary>
        /// Laplacian 方差：拉普拉斯响应的方差，除均值归一化。备用指标。
        /// </summary>
        private static double LaplacianScore(byte[] g, int w, int h)
        {
            long sumGray = 0;
            for (int i = 0; i < g.Length; i++) sumGray += g[i];
            double mean = (double)sumGray / g.Length;
            if (mean < 1e-6) mean = 1e-6;

            int n = (w - 2) * (h - 2);
            if (n <= 0) return 0;

            double sum = 0, sumSq = 0;
            for (int y = 1; y < h - 1; y++)
            {
                int row = y * w;
                for (int x = 1; x < w - 1; x++)
                {
                    int i = row + x;
                    int lap = 4 * g[i] - g[i - 1] - g[i + 1] - g[i - w] - g[i + w];
                    sum += lap;
                    sumSq += (double)lap * lap;
                }
            }
            double m = sum / n;
            double variance = sumSq / n - m * m;
            return variance / (mean * mean);
        }
    }
}