XplorePlane/XP.Calibration/Core/DualBallDetector.cs

using System.Drawing;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.Util;

namespace XP.Calibration.Core;

/// <summary>
/// 双球心检测结果
/// </summary>
public record DualBallResult
{
    /// <summary>第一个球心 (面积较大的)</summary>
    public PointF Center1 { get; init; }
    /// <summary>第二个球心 (面积较小的)</summary>
    public PointF Center2 { get; init; }
    /// <summary>两球心距离 (像素)</summary>
    public double DistancePx { get; init; }
    /// <summary>两球心距离 (mm), 需提供 pixelSize 才有效</summary>
    public double DistanceMm { get; init; }
    /// <summary>第一个球的轮廓面积</summary>
    public double Area1 { get; init; }
    /// <summary>第二个球的轮廓面积</summary>
    public double Area2 { get; init; }
}

/// <summary>
/// 双球心检测器: 从单帧图像中检测两个球心并计算距离
/// </summary>
public static class DualBallDetector
{
    /// <summary>
    /// 检测图像中两个最大球体的中心及距离
    /// </summary>
    /// <param name="srcFloat">输入图像 (CV_32F)</param>
    /// <param name="pixelSize">像素物理尺寸 (mm), 0 表示不计算物理距离</param>
    /// <param name="minArea">最小轮廓面积阈值, 过滤噪声</param>
    /// <param name="enableSubPixel">是否启用亚像素质心</param>
    public static DualBallResult? Detect(
        Mat srcFloat,
        double pixelSize = 0,
        double minArea = 50,
        bool enableSubPixel = true)
    {
        // 归一化 + 预处理
        using var img8 = new Mat();
        CvInvoke.Normalize(srcFloat, img8, 0, 255, NormType.MinMax);
        img8.ConvertTo(img8, DepthType.Cv8U);
        CvInvoke.GaussianBlur(img8, img8, new Size(3, 3), 0.8);

        // 自适应阈值
        using var bin = new Mat();
        CvInvoke.AdaptiveThreshold(img8, bin, 255,
            AdaptiveThresholdType.MeanC, ThresholdType.Binary, 31, -5);

        // 查找轮廓
        using var contours = new VectorOfVectorOfPoint();
        CvInvoke.FindContours(bin, contours, null,
            RetrType.External, ChainApproxMethod.ChainApproxNone);

        // 按面积排序, 取最大的两个
        var candidates = new List<(int Index, double Area)>();
        for (int i = 0; i < contours.Size; i++)
        {
            double area = CvInvoke.ContourArea(contours[i]);
            if (area >= minArea)
                candidates.Add((i, area));
        }

        if (candidates.Count < 2)
            return null;

        candidates.Sort((a, b) => b.Area.CompareTo(a.Area));
        var top2 = candidates.Take(2).ToList();

        // 计算两个球心
        var centers = new PointF[2];
        var areas = new double[2];

        for (int k = 0; k < 2; k++)
        {
            int idx = top2[k].Index;
            areas[k] = top2[k].Area;

            if (enableSubPixel)
            {
                using var mask = new Mat(srcFloat.Size, DepthType.Cv8U, 1);
                mask.SetTo(new MCvScalar(0));
                CvInvoke.DrawContours(mask, contours, idx, new MCvScalar(255), -1);
                centers[k] = SubPixelCentroid(srcFloat, mask);
            }
            else
            {
                var moments = CvInvoke.Moments(contours[idx]);
                centers[k] = new PointF(
                    (float)(moments.M10 / moments.M00),
                    (float)(moments.M01 / moments.M00));
            }
        }

        // 计算距离
        double dx = centers[0].X - centers[1].X;
        double dy = centers[0].Y - centers[1].Y;
        double distPx = Math.Sqrt(dx * dx + dy * dy);
        double distMm = pixelSize > 0 ? distPx * pixelSize : 0;

        return new DualBallResult
        {
            Center1 = centers[0],
            Center2 = centers[1],
            DistancePx = distPx,
            DistanceMm = distMm,
            Area1 = areas[0],
            Area2 = areas[1]
        };
    }

    /// <summary>
    /// 从 8bit 图像检测 (便捷重载, 支持 CV_8U 输入)
    /// </summary>
    public static DualBallResult? DetectFrom8U(
        Mat src8U,
        double pixelSize = 0,
        double minArea = 50,
        bool enableSubPixel = true)
    {
        using var floatMat = new Mat();
        src8U.ConvertTo(floatMat, DepthType.Cv32F);
        return Detect(floatMat, pixelSize, minArea, enableSubPixel);
    }

    private static PointF SubPixelCentroid(Mat imgFloat, Mat mask)
    {
        double sumI = 0, sumX = 0, sumY = 0;
        var imgData = imgFloat.GetData() as float[,];
        var maskData = mask.GetData() as byte[,];

        if (imgData == null || maskData == null)
            return new PointF(-1, -1);

        for (int y = 0; y < imgFloat.Rows; y++)
        {
            for (int x = 0; x < imgFloat.Cols; x++)
            {
                if (maskData[y, x] != 0)
                {
                    double intensity = imgData[y, x];
                    sumI += intensity;
                    sumX += x * intensity;
                    sumY += y * intensity;
                }
            }
        }

        if (sumI == 0)
            return new PointF(-1, -1);

        return new PointF((float)(sumX / sumI), (float)(sumY / sumI));
    }
}