增加校准通用代码

2026-04-23 19:38:44 +08:00
parent 40b229f5aa
commit b47c261bc3
12 changed files with 1329 additions and 0 deletions
@@ -0,0 +1,126 @@
+using System.Drawing;
+using Emgu.CV;
+using Emgu.CV.Util;
+using Emgu.CV.Structure;
+using XP.Calibration.Core;
+using XP.Calibration.Models;
+
+namespace XP.Calibration;
+
+/// <summary>
+/// 中心校准: 椭圆拟合 + 透视修正 | Center calibration via ellipse fitting + perspective correction
+/// </summary>
+public class CenterCalibration
+{
+    /// <summary>
+    /// 从投影序列执行中心校准
+    /// </summary>
+    /// <param name="projections">投影帧列表 (CV_32F)</param>
+    /// <param name="geo">几何参数</param>
+    /// <returns>校准结果</returns>
+    public CenterCalibrationResult Calibrate(List<Mat> projections, GeoParams geo)
+    {
+        // 检测各帧球心
+        var centers = new List<PointF>();
+        for (int i = 0; i < projections.Count; i++)
+        {
+            if (BallDetector.DetectCenter(projections[i], out var c))
+                centers.Add(c);
+        }
+
+        if (centers.Count <= 10)
+            throw new InvalidOperationException(
+                $"有效检测帧数不足: {centers.Count}, 至少需要 10 帧");
+
+        // 椭圆拟合
+        var ellipse = FitCentersEllipse(centers);
+
+        // 放大倍率
+        double M = geo.DSD / geo.DSO;
+
+        // 从椭圆参数反算倾斜角和 R
+        double ratio = ellipse.ShortAxis / ellipse.LongAxis;
+        ratio = Math.Clamp(ratio, 0.0, 1.0);
+        double alphaRad = Math.Acos(ratio);
+        double alphaDeg = alphaRad * 180.0 / Math.PI;
+
+        // 长轴 = 2 * R * M / pixelSize → R = longAxis * pixelSize / (2 * M)
+        double R = ellipse.LongAxis * geo.PixelSize / (2.0 * M);
+
+        // 透视修正
+        double deltaPx = R * R * Math.Sin(2.0 * alphaRad)
+            / (2.0 * geo.DSO * geo.DSO)
+            * geo.DSD / geo.PixelSize;
+
+        // 长轴方向角
+        var rawEllipse = FitEllipseRaw(centers);
+        double angleDeg = rawEllipse.Angle;
+        float w = rawEllipse.Size.Width;
+        float h = rawEllipse.Size.Height;
+        if (h > w) angleDeg += 90.0f;
+
+        double thetaDeg = 90.0 - angleDeg;
+        double thetaRad = thetaDeg * Math.PI / 180.0;
+
+        double deltaU = deltaPx * Math.Cos(thetaRad);
+        double deltaV = deltaPx * (-Math.Sin(thetaRad));
+
+        double u0 = ellipse.Center.X - deltaU;
+        double v0 = ellipse.Center.Y - deltaV;
+
+        return new CenterCalibrationResult
+        {
+            Ellipse = ellipse,
+            AlphaDeg = alphaDeg,
+            R_mm = R,
+            DeltaPx = deltaPx,
+            FocalU = u0,
+            FocalV = v0,
+            DetectedCenters = centers
+        };
+    }
+
+    /// <summary>
+    /// 从 RAW 文件执行中心校准 (便捷方法)
+    /// </summary>
+    public CenterCalibrationResult CalibrateFromRaw(
+        string rawPath, int width, int height, int count, GeoParams geo)
+    {
+        var projections = RawReader.ReadFloat32(rawPath, width, height, count);
+        try
+        {
+            return Calibrate(projections, geo);
+        }
+        finally
+        {
+            foreach (var p in projections) p.Dispose();
+        }
+    }
+
+    /// <summary>
+    /// 对检测到的球心序列做椭圆拟合, 返回长短轴和角度
+    /// </summary>
+    private static EllipseResult FitCentersEllipse(List<PointF> pts)
+    {
+        var rotRect = FitEllipseRaw(pts);
+        float a = rotRect.Size.Width;
+        float b = rotRect.Size.Height;
+
+        return new EllipseResult
+        {
+            Center = rotRect.Center,
+            LongAxis = Math.Max(a, b),
+            ShortAxis = Math.Min(a, b),
+            Angle = rotRect.Angle
+        };
+    }
+
+    /// <summary>
+    /// 调用 OpenCV fitEllipse 返回原始 RotatedRect
+    /// </summary>
+    private static RotatedRect FitEllipseRaw(List<PointF> pts)
+    {
+        using var vp = new VectorOfPointF(pts.ToArray());
+        return CvInvoke.FitEllipse(vp);
+    }
+}