矩形算法优化&多压力曲线支持&点位偏移设置

优化视觉定位算法，支持多压力传感器曲线显示，新增点位批量偏移设置功能。

- Management/RectangleCenterCalculator：矩形中心与角度计算切换为PCA方法，支持任意维度，提升鲁棒性，完善拟合误差计算。
- HomeViewModel：新增PressurePlotModels，支持多通道压力曲线，自动初始化，支持PNG导出。
- PlcPointParamsViewModel：新增SetOffsetCommand，支持点位批量补偿设置与对话框交互。
- 多处代码风格优化与异常处理，提升健壮性和可维护性。

TeamAAS-VM/Core/Management.cs

@@ -1834,15 +1834,17 @@ namespace TeamAAS_VP.Core
                             }
                             if (visionResults[0].IsSucceed && visionResults[1].IsSucceed && visionResults[2].IsSucceed && visionResults[3].IsSucceed)
                             {
-                                Vector<double> p1=Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
-                                Vector<double> p2=Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
-                                Vector<double> p3=Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
-                                Vector<double> p4=Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+                                
+                                Vector<double> p1=Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
+                                Vector<double> p2=Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
+                                Vector<double> p3=Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
 
-                                var center = RectangleCenterCalculator.CalculateByDirectMethod(new List<Vector<double>> { p1, p2, p4, p3 });
+                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p3, p4 });
                                 UpCameraPutResults[0].X = (float)center.Center[0];
                                 UpCameraPutResults[0].Y = (float)center.Center[1];
                                 UpCameraPutResults[0].Z= (float)center.MajorAxisAngle;
+                                SendTaskMessage($"产品拍照结果：X={UpCameraPutResults[0].X},Y={UpCameraPutResults[0].Y},Angle={UpCameraPutResults[0].Z}", MessageLevel.Info);
                             }
                             else
                             {
@@ -1921,15 +1923,17 @@ namespace TeamAAS_VP.Core
                             }
                             if (visionResults[0].IsSucceed && visionResults[1].IsSucceed && visionResults[2].IsSucceed && visionResults[3].IsSucceed)
                             {
-                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
-                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
-                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
-                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+                                
+                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
+                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
+                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
 
-                                var center = RectangleCenterCalculator.CalculateByDirectMethod(new List<Vector<double>> { p1, p2, p4, p3 });
+                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p3, p4 });
                                 UpCameraPutResults[1].X = (float)center.Center[0];
                                 UpCameraPutResults[1].Y = (float)center.Center[1];
                                 UpCameraPutResults[1].Z = (float)center.MajorAxisAngle;
+                                SendTaskMessage($"LCD拍照结果：X={UpCameraPutResults[1].X},Y={UpCameraPutResults[1].Y},Angle={UpCameraPutResults[1].Z}", MessageLevel.Info);
                             }
                             else
                             {

TeamAAS-VM/Core/RectangleCenterCalculator.cs

@@ -168,11 +168,11 @@ namespace TeamAAS_VP.Core
             // 确保短轴与长轴垂直（处理非正交情况）
             minorAxis = (minorAxis - majorAxis * minorAxis.DotProduct(majorAxis)).Normalize(2);
 
+            int dim = orderedCorners[0].Count;
             // 计算中心点（四个角点的平均）
-            var c1 = (orderedCorners[0] + orderedCorners[1] + orderedCorners[2] + orderedCorners[3])/4;
-            var center = c1;// Vector<double>.Build.DenseOfEnumerable(
-                //Enumerable.Range(0, 3)
-                   // .Select(i => orderedCorners.Select(c => c[i]).Average()));
+            var center = Vector<double>.Build.DenseOfEnumerable(
+                            Enumerable.Range(0, dim)
+                            .Select(i => orderedCorners.Select(c => c[i]).Average()));
 
             // 计算长度和宽度
             double length = Distance(topLeft, topRight);
@@ -182,7 +182,7 @@ namespace TeamAAS_VP.Core
             double angle = CalculateMajorAxisAngle(topLeft, topRight);
 
             // 计算拟合误差
-            //double fitError = CalculateFitErrorDirect(center, majorAxis, minorAxis, length, width, orderedCorners);
+            double fitError = CalculateFitErrorDirect(center, majorAxis, minorAxis, length, width, orderedCorners);
 
             return new RectangleResult
             {
@@ -194,7 +194,7 @@ namespace TeamAAS_VP.Core
                 MajorAxisAngle = angle,
                 Corners = orderedCorners,
                 Area = length * width,
-                FitError = 0//fitError
+                FitError = fitError
             };
         }
 
@@ -206,10 +206,11 @@ namespace TeamAAS_VP.Core
         {
             var orderedCorners = ValidateAndOrderCorners(corners);
 
+            int dim = orderedCorners[0].Count;
             // 步骤1：使用所有点进行PCA得到初步估计
             var matrix = Matrix<double>.Build.DenseOfRows(orderedCorners);
             var mean = Vector<double>.Build.DenseOfEnumerable(
-                Enumerable.Range(0, 3).Select(i => matrix.Column(i).Average()));
+                Enumerable.Range(0, dim).Select(i => matrix.Column(i).Average()));
 
             var centered = matrix.Clone();
             for (int i = 0; i < 4; i++)
@@ -234,7 +235,7 @@ namespace TeamAAS_VP.Core
             Vector<double> bestAxis = eigenvectors.Column(0);
             double bestSimilarity = Math.Abs(actualMajorDirection.DotProduct(bestAxis));
 
-            for (int i = 1; i < 3; i++)
+            for (int i = 1; i < eigenvectors.ColumnCount; i++)
             {
                 var axis = eigenvectors.Column(i);
                 double similarity = Math.Abs(actualMajorDirection.DotProduct(axis));
@@ -338,14 +339,28 @@ namespace TeamAAS_VP.Core
         /// </summary>
         private static Vector<double> CalculatePerpendicularAxis(Vector<double> axis)
         {
-            // 找一个不与axis共线的向量
-            Vector<double> temp;
-            if (Math.Abs(axis[0]) < 0.9)
-                temp = Vector<double>.Build.Dense(new[] { 1.0, 0.0, 0.0 });
-            else
-                temp = Vector<double>.Build.Dense(new[] { 0.0, 1.0, 0.0 });
+            if (axis == null) throw new ArgumentNullException(nameof(axis));
+            int n = axis.Count;
+            if (n == 0) throw new ArgumentException("axis must have positive dimension", nameof(axis));
+
+            // 专门处理2D：(-y, x) 是垂直向量
+            if (n == 2)
+            {
+                var perp2 = Vector<double>.Build.DenseOfArray(new[] { -axis[1], axis[0] });
+                if (perp2.L2Norm() < 1e-12) throw new ArgumentException("axis is zero vector", nameof(axis));
+                return perp2.Normalize(2);
+            }
+
+            // 一般n维：选一个与axis不共线的标准基向量
+            int idx = 0;
+            for (int i = 0; i < n; i++)
+            {
+                if (Math.Abs(axis[i]) < 0.9) { idx = i; break; }
+            }
+            var temp = Vector<double>.Build.Dense(n, i => i == idx ? 1.0 : 0.0);
 
             var perpendicular = temp - axis * temp.DotProduct(axis);
+            if (perpendicular.L2Norm() < 1e-12) throw new InvalidOperationException("failed to compute perpendicular vector");
             return perpendicular.Normalize(2);
         }
 
@@ -360,7 +375,7 @@ namespace TeamAAS_VP.Core
         /// <summary>
         /// 辅助方法：计算拟合误差
         /// </summary>
-        private static double CalculateFitError(Vector<double> center, Vector<double> majorAxis,Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
+        private static double CalculateFitError(Vector<double> center, Vector<double> majorAxis, Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
         {
             // 生成理想矩形的四个角点
             var idealCorners = new List<Vector<double>>
@@ -392,7 +407,7 @@ namespace TeamAAS_VP.Core
         /// <summary>
         /// 直接计算方法的拟合误差
         /// </summary>
-        private static double CalculateFitErrorDirect(Vector<double> center, Vector<double> majorAxis,Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
+        private static double CalculateFitErrorDirect(Vector<double> center, Vector<double> majorAxis, Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
         {
             // 对于直接方法，我们知道每个角点的理想位置
             var idealCorners = new List<Vector<double>>

TeamAAS-VM/ViewModels/HomeViewModel.cs

@@ -5,6 +5,7 @@ using OxyPlot;
 using OxyPlot.Axes;
 using OxyPlot.Legends;
 using OxyPlot.Series;
+using OxyPlot.Wpf;
 using Prism.Commands;
 using Prism.Events;
 using Prism.Ioc;
@@ -13,6 +14,7 @@ using Prism.Regions;
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using System.IO;
 using System.Linq;
 using System.Threading.Tasks;
 using System.Windows;
@@ -29,6 +31,7 @@ using TeamAAS_VP.Resources.Languages;
 using TeamAAS_VP.Services;
 using TeamAAS_VP.ViewModels.DebugMod;
 using TeamAAS_VP.Views.DebugMod;
+using static TeamAAS_VP.Models.SystemConfiguration;
 
 namespace TeamAAS_VP.ViewModels
 {
@@ -43,7 +46,7 @@ namespace TeamAAS_VP.ViewModels
 
         #region 属性
 
-        private ObservableCollection<TaskMessage> messageQueue=new ObservableCollection<TaskMessage>();
+        private ObservableCollection<TaskMessage> messageQueue = new ObservableCollection<TaskMessage>();
 
         /// <summary>
         /// 运行信息队列
@@ -51,7 +54,7 @@ namespace TeamAAS_VP.ViewModels
         public ObservableCollection<TaskMessage> MessageQueue
         {
             get { return messageQueue; }
-            set { SetProperty(ref messageQueue,value); }
+            set { SetProperty(ref messageQueue, value); }
         }
 
         private ObservableCollection<ProductModel> _ProductList;
@@ -82,14 +85,14 @@ namespace TeamAAS_VP.ViewModels
         public ProductModel SelectProduct
         {
             get { return _SelectProduct; }
-            set { SetProperty(ref _SelectProduct,value); }
+            set { SetProperty(ref _SelectProduct, value); }
         }
 
         private bool CanLoad
         {
             get
             {
-                if (SelectProduct!=null && IsLoadProduct)
+                if (SelectProduct != null && IsLoadProduct)
                 {
                     return true;
                 }
@@ -124,6 +127,16 @@ namespace TeamAAS_VP.ViewModels
             set { SetProperty(ref _CurrentProduct, value); }
         }
 
+        private ObservableCollection<PlotModel> _PressurePlotModels = new ObservableCollection<PlotModel>();
+        /// <summary>
+        /// 每个元素是一个 PlotModel，对应一个压力传感器
+        /// </summary>
+        public ObservableCollection<PlotModel> PressurePlotModels
+        {
+            get { return _PressurePlotModels; }
+            set { SetProperty(ref _PressurePlotModels, value); }
+        }
+
         private PlotModel _LockCurvePlotModel;
         /// <summary>
         /// 锁付曲线
@@ -145,17 +158,19 @@ namespace TeamAAS_VP.ViewModels
             set { SetProperty(ref _LockResults, value); }
         }
 
+        // 可选：记录传感器 Id -> 索引 的映射，便于外部（如 Management）按 Id 更新
+        private readonly Dictionary<string, int> _sensorIdToIndex = new Dictionary<string, int>();
         #endregion
 
         #region 命令
         public DelegateCommand LoadedCommand { get; set; }
         private DelegateCommand<ProductModel> _LoadProductCommand;
         public DelegateCommand<ProductModel> LoadProductCommand =>
-            _LoadProductCommand ?? (_LoadProductCommand = new DelegateCommand<ProductModel>(ExecuteLoadProductCommand).ObservesCanExecute(()=> CanLoad).ObservesProperty(()=> SelectProduct).ObservesProperty(()=> IsLoadProduct));
+            _LoadProductCommand ?? (_LoadProductCommand = new DelegateCommand<ProductModel>(ExecuteLoadProductCommand).ObservesCanExecute(() => CanLoad).ObservesProperty(() => SelectProduct).ObservesProperty(() => IsLoadProduct));
 
         private DelegateCommand _ResetCounterCommand;
         public DelegateCommand ResetCounterCommand =>
-            _ResetCounterCommand ?? (_ResetCounterCommand = new DelegateCommand(ExecuteResetCounterCommand).ObservesCanExecute(() => DoCondition).ObservesProperty(()=>isCanExecute));
+            _ResetCounterCommand ?? (_ResetCounterCommand = new DelegateCommand(ExecuteResetCounterCommand).ObservesCanExecute(() => DoCondition).ObservesProperty(() => isCanExecute));
 
         private bool DoCondition
         {
@@ -188,13 +203,23 @@ namespace TeamAAS_VP.ViewModels
             _remoteCommandService.CycleTimingTicked += _remoteCommandService_CycleTimingTicked;
             _systemDatabaseService = systemDatabaseService;
             _eventAggregator.GetEvent<LockFinishNotification>().Subscribe(OnLockFinish);
+            _eventAggregator.GetEvent<PressureFinishNotification>().Subscribe(OnPressureFinish);
+
+            //获取所有的压力传感器配置，初始化曲线图
+            var systemConfig = _configService.GetSystemConfiguration();
+            if (systemConfig.PressureSensorConfig != null)
+            {
+                InitPressurePlots(systemConfig.PressureSensorConfig.ToArray());
+            }
+
+
         }
 
         private void _remoteCommandService_CycleTimingTicked(object sender, TimeSpan e)
         {
             try
             {
-                if (App.Current!=null)
+                if (App.Current != null)
                 {
                     App.Current.Invoke(() =>
                     {
@@ -209,7 +234,7 @@ namespace TeamAAS_VP.ViewModels
         }
 
         #region 方法
-        bool isFirst=true;
+        bool isFirst = true;
         private void OnLoad()
         {
             if (!isFirst)
@@ -342,7 +367,7 @@ namespace TeamAAS_VP.ViewModels
 
         private void LoginChange(Models.User user)
         {
-            
+
         }
 
         /// <summary>
@@ -368,7 +393,7 @@ namespace TeamAAS_VP.ViewModels
         //初始化曲线图
         private void InitLockCurvePlotModel()
         {
-            var model = new PlotModel { Title = "锁付曲线", TitleFontSize = 16};
+            var model = new PlotModel { Title = "锁付曲线", TitleFontSize = 16 };
             // 设置图例
             var legend = new Legend
             {
@@ -381,7 +406,7 @@ namespace TeamAAS_VP.ViewModels
             // 设置X轴
             var xAxis = new LinearAxis
             {
-                Key="X",
+                Key = "X",
                 Position = AxisPosition.Bottom,
                 Title = "角度 (°)",
                 Minimum = 0,
@@ -437,9 +462,106 @@ namespace TeamAAS_VP.ViewModels
                     series.Points.Add(new OxyPlot.DataPoint(waveData.LockAngle, waveData.Torque));
                 }
                 LockCurvePlotModel.InvalidatePlot(true);
+
             }));
 
         }
+
+        /// <summary>
+        /// 保压完成时调用
+        /// </summary>
+        /// <param name="tuple"></param>
+        private void OnPressureFinish((bool IsSuccess, int Index, (DateTime Timestamp, float Value)[] values, string SavePath) tuple)
+        {
+            //更新对应传感器的曲线图
+            if (_sensorIdToIndex.TryGetValue(tuple.Index.ToString(), out int index))
+            {
+                App.Current.Dispatcher.BeginInvoke(new Action(() => {
+                    var model = PressurePlotModels[index];
+                    var series = model.Series[0] as LineSeries;
+                    series.Points.Clear();
+                    //第一个点的时间作为X轴起点
+                    DateTime xAxisStart = tuple.values[0].Timestamp;
+                    foreach (var dataPoint in tuple.values)
+                    {
+                        // 计算相对于起点的秒数作为X轴坐标
+                        double xValue = (dataPoint.Timestamp - xAxisStart).TotalSeconds;
+                        series.Points.Add(new OxyPlot.DataPoint(xValue, dataPoint.Value));
+                    }
+                    model.InvalidatePlot(true);
+                    if (!string.IsNullOrEmpty(tuple.SavePath))
+                    {
+                        if (!Directory.Exists(Path.GetDirectoryName(tuple.SavePath)))
+                        {
+                            Directory.CreateDirectory(Path.GetDirectoryName(tuple.SavePath));
+                        }
+                        PngExporter.Export(model, tuple.SavePath, (int)model.Width, (int)model.Height);
+                    }
+                }));
+            }
+        }
+
+        // 初始化由配置决定的曲线数量（在读取 SystemConfiguration 后调用）
+        public void InitPressurePlots(IEnumerable<PressureSensorSettings> sensors)
+        {
+            PressurePlotModels.Clear();
+            _sensorIdToIndex.Clear();
+
+            int idx = 0;
+            foreach (var s in sensors)
+            {
+                var model = CreatePlotModelForSensor(s);
+                PressurePlotModels.Add(model);
+                // 假设 PressureSensorConfig 有唯一 Id（string 或 int）
+                _sensorIdToIndex[s.SensorId.ToString()] = idx++;
+            }
+        }
+
+        private PlotModel CreatePlotModelForSensor(PressureSensorSettings sensor)
+        {
+            var model = new PlotModel { Title = sensor.SensorName ?? $"Sensor {sensor.SensorId}", TitleFontSize = 16 };
+            // 设置图例
+            var legend = new Legend
+            {
+                LegendPosition = LegendPosition.TopRight,
+                LegendPlacement = LegendPlacement.Outside,
+                LegendOrientation = LegendOrientation.Vertical,
+                LegendBorderThickness = 0
+            };
+            model.Legends.Add(legend);
+            // 设置X轴
+            var xAxis = new LinearAxis
+            {
+                Key = "X",
+                Position = AxisPosition.Bottom,
+                Title = "Time (s)",
+                Minimum = 0,
+                MajorGridlineStyle = LineStyle.Solid,
+                MinorGridlineStyle = LineStyle.Dot
+            };
+            model.Axes.Add(xAxis);
+            // 设置Y轴
+            var yAxis = new LinearAxis
+            {
+                Key = "Y",
+                Position = AxisPosition.Left,
+                Title = "Pressure",
+                MajorGridlineStyle = LineStyle.Solid,
+                MinorGridlineStyle = LineStyle.Dot
+            };
+            model.Axes.Add(yAxis);
+            // 添加示例数据系列
+            var series = new LineSeries
+            {
+                Title = sensor.SensorName ?? $"Sensor {sensor.SensorId}",
+                StrokeThickness = 2,
+                MarkerSize = 3,
+                MarkerType = MarkerType.Circle,
+                CanTrackerInterpolatePoints = false,
+            };
+            model.Series.Add(series);
+            return model;
+        }
         #endregion
 
         #region 继承

TeamAAS-VM/ViewModels/Product/PlcPointParamsViewModel.cs

@@ -433,6 +433,9 @@ namespace TeamAAS_VP.ViewModels.Product
         public DelegateCommand DynamicTestAnalyzerCommand =>
             _DynamicTestAnalyzerCommand ?? (_DynamicTestAnalyzerCommand = new DelegateCommand(ExecuteDynamicTestAnalyzerCommand));
 
+        private DelegateCommand _SetOffsetCommand;
+        public DelegateCommand SetOffsetCommand =>
+            _SetOffsetCommand ?? (_SetOffsetCommand = new DelegateCommand(ExecuteSetOffsetCommand));
         #endregion
 
         #region 事件
@@ -1094,6 +1097,74 @@ namespace TeamAAS_VP.ViewModels.Product
             }
         }
 
+        /// <summary>
+        /// 设置偏移
+        /// </summary>
+        void ExecuteSetOffsetCommand()
+        {
+            if (SelectedIndex < 0) return;
+            IDialogParameters parameters1 = new DialogParameters();
+            if (SelectedIndex == 0)
+            {
+                parameters1.Add("Title", "取料拍照点位-补偿值设置");
+            }
+            else if (SelectedIndex == 1)
+            {
+                parameters1.Add("Title", "取料点位-补偿值设置");
+            }
+            else if (SelectedIndex == 2)
+            {
+                parameters1.Add("Title", "下相机二次定位点位-补偿值设置");
+            }
+            else if (SelectedIndex == 3)
+            {
+                parameters1.Add("Title", "锁附拍照点位-补偿值设置");
+            }
+            else if (SelectedIndex == 4)
+            {
+                parameters1.Add("Title", "锁附点位-补偿值设置");
+            }
+            else if (SelectedIndex == 5)
+            {
+                parameters1.Add("Title", "复检点位-补偿值设置");
+            }
+            else
+            {
+                parameters1.Add("Title", "点位-补偿值设置");
+            }
+
+            List<PlcPoint> _points = new List<PlcPoint>();
+            foreach (var item in Points)
+            {
+                _points.Add(item.Clone());
+            }
+            parameters1.Add("Points", _points.ToArray());
+            _dialogService.ShowDialog("PlcPointOffserParams", parameters1, rst =>
+            {
+                //对话框关闭之后的回调函数，可以在这解析结果。
+                ButtonResult result1 = rst.Result;
+
+                if (result1 == ButtonResult.OK)
+                {
+                    var param = rst.Parameters.GetValue<PlcPoint[]>("Points");
+
+                    //更新点位
+                    for (int i = 0; i < Points.Count; i++)
+                    {
+                        Points[i].X_Offset = param[i].X_Offset;
+                        Points[i].Y_Offset = param[i].Y_Offset;
+                        Points[i].Z_Start_Offset = param[i].Z_Start_Offset;
+                        Points[i].Z_Stop_Offset = param[i].Z_Stop_Offset;
+                        Points[i].U_Offset = param[i].U_Offset;
+                        Points[i].R_Offset = param[i].R_Offset;
+                    }
+
+                    _eventAggregator.GetEvent<SnackbarMessageNotification>().Publish(new MessageParameter() { Msg = $"{Lang.完成}", Duration = 1 });
+                }
+
+            });
+        }
+
         /// <summary>
         /// 停止采集
         /// </summary>