XplorePlane/XP.Camera/Calibration/CalibrationProcessor.cs

// ============================================================================
// 文件名: CalibrationProcessor.cs
// 描述: 标定处理器，实现图像坐标系到世界坐标系的转换
// 功能:
//   - 基于多点标定计算透视变换矩阵（支持4点及以上）
//   - 像素坐标到世界坐标的转换
//   - 标定数据的保存和加载（JSON格式）
//   - 从CSV文件导入标定点数据
// 算法: 使用DLT（Direct Linear Transformation）方法求解单应性矩阵
// ============================================================================

using Emgu.CV;
using System.IO;
using System.Text.Json;

namespace XP.Camera.Calibration;

/// <summary>
/// 九点标定处理器
/// </summary>
public class CalibrationProcessor
{
    public class CalibrationPoint
    {
        public double PixelX { get; set; }
        public double PixelY { get; set; }
        public double WorldX { get; set; }
        public double WorldY { get; set; }
    }

    public class CalibrationData
    {
        public List<CalibrationPoint> Points { get; set; } = new List<CalibrationPoint>();
        public double[] TransformMatrix { get; set; } = new double[9];
        public DateTime CalibrationTime { get; set; }
    }

    private Matrix<double>? _transformMatrix;

    /// <summary>
    /// 执行九点标定
    /// </summary>
    public bool Calibrate(List<CalibrationPoint> points)
    {
        if (points.Count < 4)
            return false;

        int n = points.Count;
        var A = new Matrix<double>(2 * n, 8);
        var b = new Matrix<double>(2 * n, 1);

        for (int i = 0; i < n; i++)
        {
            double u = points[i].PixelX;
            double v = points[i].PixelY;
            double x = points[i].WorldX;
            double y = points[i].WorldY;

            A[2 * i, 0] = u;
            A[2 * i, 1] = v;
            A[2 * i, 2] = 1;
            A[2 * i, 3] = 0;
            A[2 * i, 4] = 0;
            A[2 * i, 5] = 0;
            A[2 * i, 6] = -x * u;
            A[2 * i, 7] = -x * v;
            b[2 * i, 0] = x;

            A[2 * i + 1, 0] = 0;
            A[2 * i + 1, 1] = 0;
            A[2 * i + 1, 2] = 0;
            A[2 * i + 1, 3] = u;
            A[2 * i + 1, 4] = v;
            A[2 * i + 1, 5] = 1;
            A[2 * i + 1, 6] = -y * u;
            A[2 * i + 1, 7] = -y * v;
            b[2 * i + 1, 0] = y;
        }

        var h = new Matrix<double>(8, 1);
        CvInvoke.Solve(A, b, h, Emgu.CV.CvEnum.DecompMethod.Svd);

        _transformMatrix = new Matrix<double>(3, 3);
        _transformMatrix[0, 0] = h[0, 0];
        _transformMatrix[0, 1] = h[1, 0];
        _transformMatrix[0, 2] = h[2, 0];
        _transformMatrix[1, 0] = h[3, 0];
        _transformMatrix[1, 1] = h[4, 0];
        _transformMatrix[1, 2] = h[5, 0];
        _transformMatrix[2, 0] = h[6, 0];
        _transformMatrix[2, 1] = h[7, 0];
        _transformMatrix[2, 2] = 1.0;

        return true;
    }

    /// <summary>
    /// 像素坐标转世界坐标
    /// </summary>
    public System.Drawing.PointF PixelToWorld(System.Drawing.PointF pixel)
    {
        if (_transformMatrix == null)
            return pixel;

        double u = pixel.X;
        double v = pixel.Y;

        double w = _transformMatrix[2, 0] * u + _transformMatrix[2, 1] * v + _transformMatrix[2, 2];
        double x = (_transformMatrix[0, 0] * u + _transformMatrix[0, 1] * v + _transformMatrix[0, 2]) / w;
        double y = (_transformMatrix[1, 0] * u + _transformMatrix[1, 1] * v + _transformMatrix[1, 2]) / w;

        return new System.Drawing.PointF((float)x, (float)y);
    }

    /// <summary>
    /// 保存标定文件
    /// </summary>
    public void SaveCalibration(string filePath, List<CalibrationPoint> points)
    {
        var data = new CalibrationData
        {
            Points = points,
            TransformMatrix = new double[9],
            CalibrationTime = DateTime.Now
        };

        if (_transformMatrix != null)
        {
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    data.TransformMatrix[i * 3 + j] = _transformMatrix[i, j];
        }

        var json = JsonSerializer.Serialize(data, new JsonSerializerOptions { WriteIndented = true });
        File.WriteAllText(filePath, json);
    }

    /// <summary>
    /// 加载标定文件
    /// </summary>
    public bool LoadCalibration(string filePath)
    {
        if (!File.Exists(filePath))
            return false;

        try
        {
            var json = File.ReadAllText(filePath);
            var data = JsonSerializer.Deserialize<CalibrationData>(json);

            if (data == null || data.TransformMatrix == null || data.TransformMatrix.Length != 9)
                return false;

            _transformMatrix = new Matrix<double>(3, 3);
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    _transformMatrix[i, j] = data.TransformMatrix[i * 3 + j];

            return true;
        }
        catch
        {
            return false;
        }
    }

    /// <summary>
    /// 从CSV文件加载标定点
    /// </summary>
    public List<CalibrationPoint> LoadPointsFromCsv(string filePath)
    {
        var points = new List<CalibrationPoint>();
        if (!File.Exists(filePath))
            return points;

        var lines = File.ReadAllLines(filePath);
        for (int i = 0; i < lines.Length; i++)
        {
            if (i == 0 && (lines[i].Contains("PixelX") || lines[i].Contains("像素"))) continue;

            var parts = lines[i].Split(',');
            if (parts.Length >= 4)
            {
                if (double.TryParse(parts[0].Trim(), out double px) &&
                    double.TryParse(parts[1].Trim(), out double py) &&
                    double.TryParse(parts[2].Trim(), out double wx) &&
                    double.TryParse(parts[3].Trim(), out double wy))
                {
                    points.Add(new CalibrationPoint
                    {
                        PixelX = px,
                        PixelY = py,
                        WorldX = wx,
                        WorldY = wy
                    });
                }
            }
        }
        return points;
    }
}