TeamAAS-2.5/TeamAAS.Plugins/Plugins.Vpp/Views/VppCalibrationPage.xaml.cs

using System;
using System.Collections.ObjectModel;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using Microsoft.Win32;

namespace Plugins.Vpp.Engines
{
    /// <summary>标定点位行</summary>
    public class CalibrationPointItem
    {
        public int Index { get; set; }
        public double RobotX { get; set; }
        public double RobotY { get; set; }
        public double PixelX { get; set; }
        public double PixelY { get; set; }
    }

    /// <summary>
    /// VisionPro 引擎标定窗体：
    /// - 九点标定：像素 → 机器人 仿射变换（robotX=a·px+b·py+c，robotY=d·px+e·py+f，最小二乘）；
    /// - 旋转中心：像素坐标圆拟合（Kasa 最小二乘）。
    /// 自包含实现，不依赖外部数学库；仿射系数随结果输出，供宿主换算使用。
    /// </summary>
    public partial class VppCalibrationPage : UserControl
    {
        private bool _isRotationCenter;

        public ObservableCollection<CalibrationPointItem> Points { get; }
            = new ObservableCollection<CalibrationPointItem>();

        public VppCalibrationPage()
        {
            InitializeComponent();
            DataContext = this;
            ResetPoints(9);
        }

        private void ResetPoints(int count)
        {
            Points.Clear();
            for (int i = 0; i < count; i++)
                Points.Add(new CalibrationPointItem { Index = i + 1 });
        }

        private void NinePoint_Checked(object sender, RoutedEventArgs e) { _isRotationCenter = false; ResetPoints(9); }
        private void RotationCenter_Checked(object sender, RoutedEventArgs e) { _isRotationCenter = true; ResetPoints(4); }

        private void AddRow_Click(object sender, RoutedEventArgs e)
            => Points.Add(new CalibrationPointItem { Index = Points.Count + 1 });

        private void ClearPoints_Click(object sender, RoutedEventArgs e)
            => ResetPoints(_isRotationCenter ? 4 : 9);

        private void LoadImage_Click(object sender, RoutedEventArgs e)
        {
            var dlg = new OpenFileDialog { Filter = "图片文件|*.bmp;*.png;*.jpg;*.jpeg;*.tif;*.tiff" };
            if (dlg.ShowDialog() != true) return;
            var bmp = new BitmapImage();
            bmp.BeginInit();
            bmp.CacheOption = BitmapCacheOption.OnLoad;
            bmp.UriSource = new Uri(dlg.FileName);
            bmp.EndInit();
            bmp.Freeze();
            CalibImage.Source = bmp;
            ImagePlaceholder.Visibility = Visibility.Collapsed;
        }

        private void Calculate_Click(object sender, RoutedEventArgs e)
        {
            try
            {
                ResultText.Text = _isRotationCenter ? CalculateRotationCenter() : CalculateNinePoint();
            }
            catch (Exception ex)
            {
                ResultText.Text = "标定计算失败：" + ex.Message;
            }
        }

        /// <summary>九点标定：像素 → 机器人 仿射最小二乘</summary>
        private string CalculateNinePoint()
        {
            int n = 0;
            // 正规方程累加：M = Σ[px² pxpy px; pxpy py² py; px py 1]，bX = Σ[px·rx py·rx rx]，bY 同理
            double m11 = 0, m12 = 0, m13 = 0, m22 = 0, m23 = 0, m33 = 0;
            double bx1 = 0, bx2 = 0, bx3 = 0, by1 = 0, by2 = 0, by3 = 0;

            foreach (var p in Points)
            {
                if (p.PixelX == 0 && p.PixelY == 0) continue;
                n++;
                m11 += p.PixelX * p.PixelX; m12 += p.PixelX * p.PixelY; m13 += p.PixelX;
                m22 += p.PixelY * p.PixelY; m23 += p.PixelY;
                m33 += 1;
                bx1 += p.PixelX * p.RobotX; bx2 += p.PixelY * p.RobotX; bx3 += p.RobotX;
                by1 += p.PixelX * p.RobotY; by2 += p.PixelY * p.RobotY; by3 += p.RobotY;
            }
            if (n < 3) throw new InvalidOperationException("有效标定点不足 3 个");

            // [a b c] = M⁻¹ · bX（robotX = a·px + b·py + c）
            var abc = Solve3(m11, m12, m13, m12, m22, m23, m13, m23, m33, bx1, bx2, bx3);
            // [d e f] = M⁻¹ · bY（robotY = d·px + e·py + f）
            var def = Solve3(m11, m12, m13, m12, m22, m23, m13, m23, m33, by1, by2, by3);

            // 复验误差（最大偏差）
            double maxErr = 0;
            foreach (var p in Points)
            {
                if (p.PixelX == 0 && p.PixelY == 0) continue;
                var rx = abc[0] * p.PixelX + abc[1] * p.PixelY + abc[2];
                var ry = def[0] * p.PixelX + def[1] * p.PixelY + def[2];
                maxErr = Math.Max(maxErr, Math.Sqrt(Math.Pow(rx - p.RobotX, 2) + Math.Pow(ry - p.RobotY, 2)));
            }

            return $"九点标定完成（{n} 点）\r\n" +
                   $"像素→机器人 仿射系数：\r\n" +
                   $"robotX = {abc[0]:F6}·px + {abc[1]:F6}·py + {abc[2]:F3}\r\n" +
                   $"robotY = {def[0]:F6}·px + {def[1]:F6}·py + {def[2]:F3}\r\n" +
                   $"最大重投影误差：{maxErr:F4} mm";
        }

        /// <summary>3×3 线性方程组高斯消元求解</summary>
        private static double[] Solve3(
            double m11, double m12, double m13,
            double m21, double m22, double m23,
            double m31, double m32, double m33,
            double b1, double b2, double b3)
        {
            double det = m11 * (m22 * m33 - m23 * m32)
                       - m12 * (m21 * m33 - m23 * m31)
                       + m13 * (m21 * m32 - m22 * m31);
            if (Math.Abs(det) < 1e-12)
                throw new InvalidOperationException("标定点位分布退化（共线），无法求解");

            return new[]
            {
                (b1 * (m22 * m33 - m23 * m32) - m12 * (b1 * m33 - m23 * b3) + m13 * (b1 * m32 - m22 * b3)) / det,
                (m11 * (b2 * m33 - m23 * b3) - b1 * (m21 * m33 - m23 * m31) + m13 * (m21 * b3 - b2 * m31)) / det,
                (m11 * (m22 * b3 - b2 * m32) - m12 * (m21 * b3 - b2 * m31) + b1 * (m21 * m32 - m22 * m31)) / det,
            };
        }

        /// <summary>旋转中心：像素坐标圆拟合（Kasa 最小二乘）</summary>
        private string CalculateRotationCenter()
        {
            int n = 0;
            double sx = 0, sy = 0, sxx = 0, syy = 0, sxy = 0, sxz = 0, syz = 0, sz = 0;
            foreach (var p in Points)
            {
                if (p.PixelX == 0 && p.PixelY == 0) continue;
                double z = p.PixelX * p.PixelX + p.PixelY * p.PixelY;
                n++;
                sx += p.PixelX; sy += p.PixelY;
                sxx += p.PixelX * p.PixelX; syy += p.PixelY * p.PixelY; sxy += p.PixelX * p.PixelY;
                sxz += p.PixelX * z; syz += p.PixelY * z; sz += z;
            }
            if (n < 3) throw new InvalidOperationException("有效标定点不足 3 个");

            // [sxx sxy; sxy syy][a; b] = [sxz; syz]，圆心=(a/2, b/2)，r=√(c+a²+b²)
            double det = sxx * syy - sxy * sxy;
            if (Math.Abs(det) < 1e-9) throw new InvalidOperationException("点位共线，无法拟合圆");
            double a = (sxz * syy - syz * sxy) / det;
            double b = (sxx * syz - sxy * sxz) / det;
            double c = (sz - a * sx - b * sy) / n;
            double r = Math.Sqrt(Math.Max(0, c + a * a + b * b));

            return $"旋转中心标定完成（{n} 点）\r\n" +
                   $"旋转中心（像素）：({a / 2:F2}, {b / 2:F2})\r\n" +
                   $"旋转半径：{r:F2} px";
        }
    }
}