Display_D_HP/TeamAAS-VM/Services/CheckerboardDetectionService.cs

using TeamAAS_VP.Models;
using OpenCvSharp;
using System;
using System.Collections.Generic;
using System.Linq;

namespace TeamAAS_VP.Services
{
    /// <summary>
    /// 棋盘格自动检测服务
    /// </summary>
    public class CheckerboardDetectionService
    {
        /// <summary>
        /// 自动检测棋盘格（整图）
        /// </summary>
        /// <param name="image">输入图像</param>
        /// <returns>棋盘格信息</returns>
        public ChessboardInfo DetectChessboard(Mat image)
        {
            var info = new ChessboardInfo { IsDetected = false };

            if (image == null || image.Empty())
                return info;

            try
            {
                // 转换为灰度图
                Mat gray = new Mat();
                if (image.Channels() == 3)
                {
                    Cv2.CvtColor(image, gray, ColorConversionCodes.BGR2GRAY);
                }
                else
                {
                    gray = image.Clone();
                }

                // 尝试不同的棋盘格尺寸
                var possibleSizes = GeneratePossibleSizes(3, 15);

                foreach (var size in possibleSizes)
                {
                    Point2f[] corners;

                    bool found = Cv2.FindChessboardCornersSB(
                        gray,
                        size,
                        out corners,
                        ChessboardFlags.None);

                    if (found && corners != null && corners.Length > 0)
                    {
                        // 亚像素精度优化
                        var criteria = new TermCriteria(CriteriaTypes.Eps | CriteriaTypes.MaxIter, 30, 0.001);
                        Cv2.CornerSubPix(gray, corners, new Size(11, 11), new Size(-1, -1), criteria);

                        // 计算置信度
                        double confidence = CalculateConfidence(gray, corners, size);

                        // 只接受高置信度的检测结果
                        if (confidence > 0.7)
                        {
                            info.IsDetected = true;
                            info.CornersWidth = size.Width;
                            info.CornersHeight = size.Height;
                            info.Corners = corners;
                            info.Confidence = confidence;
                            info.BoundingRect = CalculateBoundingRect(corners);
                            break;
                        }
                    }
                }

                gray?.Dispose();
            }
            catch (Exception ex)
            {
                Console.WriteLine($"棋盘格检测错误: {ex.Message}");
            }

            return info;
        }

        /// <summary>
        /// 在指定旋转的 ROI 区域中检测棋盘格（支持旋转矩形）
        /// roi: ROI 的像素矩形（外接矩形）
        /// angleDegrees: ROI 相对于图像的旋转角度（度）
        /// </summary>
        public ChessboardInfo DetectChessboard(Mat image, OpenCvSharp.Rect roi, double angleDegrees)
        {
            var info = new ChessboardInfo { IsDetected = false };
            if (image == null || image.Empty())
                return info;

            try
            {
                // 提取旋转裁剪图像
                Point2f cropOrigin, rotationCenter;
                using (var crop = ExtractRotatedRoi(image, roi, angleDegrees, out cropOrigin, out rotationCenter))
                {
                    if (crop == null || crop.Empty())
                        return info;

                    // 在裁剪图像上检测（整图检测函数会尝试多种规格）
                    var subInfo = DetectChessboard(crop);
                    if (subInfo == null || !subInfo.IsDetected)
                        return info;

                    // 将裁剪图像中的角点映射回原图坐标
                    var mapped = MapCornersFromRotatedToOriginal(subInfo.Corners, cropOrigin, rotationCenter, angleDegrees);
                    info.IsDetected = true;
                    info.Corners = mapped;
                    info.CornersWidth = subInfo.CornersWidth;
                    info.CornersHeight = subInfo.CornersHeight;
                    info.Confidence = subInfo.Confidence;

                    // 重新计算边界
                    info.BoundingRect = CalculateBoundingRect(info.Corners);
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine($"旋转ROI检测错误: {ex.Message}");
            }

            return info;
        }

        /// <summary>
        /// 从原图中提取旋转后的ROI（将图像绕ROI中心反向旋转后裁剪）
        /// 返回裁剪图像，并通过out参数返回裁剪在旋转后图像中的原点和旋转中心
        /// </summary>
        public Mat ExtractRotatedRoi(Mat src, OpenCvSharp.Rect roi, double angleDegrees, out Point2f cropOrigin, out Point2f rotationCenter)
        {
            cropOrigin = new Point2f(0, 0);
            rotationCenter = new Point2f(0, 0);

            if (src == null || src.Empty())
                return null;

            if (roi.Width <= 0 || roi.Height <= 0)
                return null;

            // compute center of ROI in source image coords
            float cx = roi.X + roi.Width / 2.0f;
            float cy = roi.Y + roi.Height / 2.0f;
            rotationCenter = new Point2f(cx, cy);

            // rotate entire image by -angle around center so that the rotated ROI becomes axis-aligned
            Mat rotated = new Mat();
            var M = Cv2.GetRotationMatrix2D(new Point2f(cx, cy), (float)-angleDegrees, 1.0);
            Cv2.WarpAffine(src, rotated, M, new Size(src.Width, src.Height), InterpolationFlags.Linear, BorderTypes.Constant, Scalar.All(0));

            // after rotation the ROI center remains at (cx,cy); crop axis-aligned rectangle with same width/height
            int x = (int)Math.Round(cx - roi.Width / 2.0);
            int y = (int)Math.Round(cy - roi.Height / 2.0);

            // clamp
            x = Math.Max(0, Math.Min(x, rotated.Width - 1));
            y = Math.Max(0, Math.Min(y, rotated.Height - 1));
            int w = Math.Min(roi.Width, rotated.Width - x);
            int h = Math.Min(roi.Height, rotated.Height - y);

            if (w <= 0 || h <= 0)
            {
                rotated.Dispose();
                return null;
            }

            cropOrigin = new Point2f(x, y);
            var crop = new Mat(rotated, new OpenCvSharp.Rect(x, y, w, h)).Clone();
            rotated.Dispose();
            return crop;
        }

        /// <summary>
        /// 将在旋转后裁剪图像中检测到的角点映射回原图像坐标
        /// </summary>
        public Point2f[] MapCornersFromRotatedToOriginal(Point2f[] cornersInCrop, Point2f cropOrigin, Point2f rotationCenter, double angleDegrees)
        {
            if (cornersInCrop == null || cornersInCrop.Length == 0)
                return cornersInCrop;

            var mapped = new Point2f[cornersInCrop.Length];
            double angleRad = angleDegrees * Math.PI / 180.0;
            double cos = Math.Cos(angleRad);
            double sin = Math.Sin(angleRad);

            for (int i = 0; i < cornersInCrop.Length; i++)
            {
                // position in rotated image (absolute)
                double rx = cropOrigin.X + cornersInCrop[i].X;
                double ry = cropOrigin.Y + cornersInCrop[i].Y;

                // translate to rotation center
                double dx = rx - rotationCenter.X;
                double dy = ry - rotationCenter.Y;

                // rotate back by +angle to original image coordinates
                double ox = cos * dx - sin * dy + rotationCenter.X;
                double oy = sin * dx + cos * dy + rotationCenter.Y;

                mapped[i] = new Point2f((float)ox, (float)oy);
            }

            return mapped;
        }

        /// <summary>
        /// 生成可能的棋盘格尺寸
        /// </summary>
        private List<Size> GeneratePossibleSizes(int minSize, int maxSize)
        {
            var sizes = new List<Size>();

            // 常见的棋盘格尺寸
            var commonSizes = new[]
            {
                new Size(9, 6),   // 标准标定板
                new Size(8, 6),
                new Size(10, 7),
                new Size(11, 8),
                new Size(9, 7),
                new Size(7, 5),
                new Size(6, 4),
                new Size(8, 5),
            };

            sizes.AddRange(commonSizes);

            // 添加其他可能的尺寸
            for (int w = minSize; w <= maxSize; w++)
            {
                for (int h = minSize; h <= maxSize; h++)
                {
                    var size = new Size(w, h);
                    if (!sizes.Any(s => s.Width == w && s.Height == h))
                    {
                        sizes.Add(size);
                    }
                }
            }

            return sizes;
        }

        /// <summary>
        /// 计算检测置信度
        /// </summary>
        private double CalculateConfidence(Mat gray, Point2f[] corners, Size patternSize)
        {
            if (corners == null || corners.Length == 0)
                return 0;

            double confidence = 0;

            try
            {
                // 1. 检查角点的规律性 (通过计算角点间距的标准差)
                var distances = new List<double>();
                for (int i = 0; i < corners.Length - 1; i++)
                {
                    double dist = Math.Sqrt(
                        Math.Pow(corners[i + 1].X - corners[i].X, 2) +
                        Math.Pow(corners[i + 1].Y - corners[i].Y, 2));
                    distances.Add(dist);
                }

                if (distances.Count > 0)
                {
                    double avgDist = distances.Average();
                    double variance = distances.Sum(d => Math.Pow(d - avgDist, 2)) / distances.Count;
                    double stdDev = Math.Sqrt(variance);
                    
                    // 标准差越小，规律性越好
                    double regularityScore = Math.Max(0, 1 - (stdDev / avgDist));
                    confidence += regularityScore * 0.5;
                }

                // 2. 检查角点数量是否完全匹配
                int expectedCorners = patternSize.Width * patternSize.Height;
                if (corners.Length == expectedCorners)
                {
                    confidence += 0.3;
                }

                // 3. 检查角点是否在图像范围内
                int validCorners = corners.Count(c => 
                    c.X >= 0 && c.X < gray.Width && 
                    c.Y >= 0 && c.Y < gray.Height);
                
                confidence += (validCorners / (double)corners.Length) * 0.2;
            }
            catch
            {
                confidence = 0.5; // 默认中等置信度
            }

            return Math.Min(1.0, confidence);
        }

        /// <summary>
        /// 计算边界矩形
        /// </summary>
        private OpenCvSharp.Rect CalculateBoundingRect(Point2f[] corners)
        {
            if (corners == null || corners.Length == 0)
                return new OpenCvSharp.Rect();

            float minX = corners.Min(c => c.X);
            float minY = corners.Min(c => c.Y);
            float maxX = corners.Max(c => c.X);
            float maxY = corners.Max(c => c.Y);

            return new OpenCvSharp.Rect(
                (int)minX,
                (int)minY,
                (int)(maxX - minX),
                (int)(maxY - minY));
        }

        /// <summary>
        /// 在图像上绘制棋盘格角点
        /// </summary>
        public void DrawChessboard(Mat image, ChessboardInfo info)
        {
            if (image == null || !info.IsDetected || info.Corners == null)
                return;

            try
            {
                var patternSize = new Size(info.CornersWidth, info.CornersHeight);
                Cv2.DrawChessboardCorners(image, patternSize, info.Corners, true);

                // 绘制边界矩形
                Cv2.Rectangle(image, info.BoundingRect, new Scalar(0, 255, 0), 2);

                // 显示信息
                string text = $"{info.CornersWidth}x{info.CornersHeight} ({info.Confidence:F2})";
                Cv2.PutText(image, text, new Point(10, 30), 
                    HersheyFonts.HersheySimplex, 0.8, new Scalar(0, 255, 0), 2);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"绘制棋盘格错误: {ex.Message}");
            }
        }
    }
}