相机与视觉算法优化，增强兼容性与示教功能

本次提交主要内容：
- 相机采集策略改为OneByOne，曝光/增益设置更鲁棒，采集后清空缓冲区
- KCS光源控制器兼容更多返回格式，提升设备适配性
- 视觉定位算法由PCA切换为直接法，提升中心点与角度计算一致性
- 工具坐标与放料点位计算逻辑优化，统一使用上相机结果
- 标定流程自动化，增加机器人-相机旋转矩阵识别与自动居中
- 远程命令服务增加曝光/增益设置反馈，代码结构优化
- 点位示教支持新模式，弹窗确认并控制附加轴
- 其他：引入新依赖，代码格式与注释优化

整体提升了系统的兼容性、精度、自动化和用户体验。

TeamAAS-VM/Core/Cameras/OptCamera.cs

@@ -353,7 +353,7 @@ namespace TeamAAS_VP.Core.Cameras
                 // ch:注册事件回调 | en:Register event callback
                 //nReVal = mvCameraAcq.RegisterPayloadCallBack(new SciCam.fnOnPayloadDelegate(ImageCallBack), IntPtr.Zero, true);
                 // ch:设置采集策略 | en:Set Grab Strategy
-                nReVal = mvCameraAcq.SetGrabStrategy(SciCam.SciCamGrabStrategy.SciCam_GrabStrategy_Latest);
+                nReVal = mvCameraAcq.SetGrabStrategy(SciCam.SciCamGrabStrategy.SciCam_GrabStrategy_OneByOne);
                 if (nReVal != SciCam.SCI_CAMERA_OK)
                 {
                     Console.WriteLine("Register Event CallBack fail! nRet {0}", nReVal);
@@ -547,11 +547,15 @@ namespace TeamAAS_VP.Core.Cameras
                 if (!mvCameraAcq.IsDeviceOpen())
                     return false;
                 uint nReVal = SciCam.SCI_CAMERA_OK;
-                nReVal = mvCameraAcq.SetIntValue("ExposureTime", (int)ExposureTime);
-                if (nReVal != SciCam.SCI_CAMERA_OK)
+                for (int i = 0; i < 3; i++)
                 {
-                    mvCameraAcq.SetFloatValue("ExposureTime", ExposureTime);
+                    nReVal = mvCameraAcq.SetIntValue("ExposureTime", (int)ExposureTime);
+                    if (nReVal != SciCam.SCI_CAMERA_OK)
+                    {
+                        mvCameraAcq.SetFloatValue("ExposureTime", ExposureTime);
+                    }
                 }
+                mvCameraAcq.ClearPayloadBuffer();
                 return true;
             }
             finally
@@ -612,10 +616,13 @@ namespace TeamAAS_VP.Core.Cameras
                 if (!mvCameraAcq.IsDeviceOpen())
                     return false;
                 uint nReVal = SciCam.SCI_CAMERA_OK;
-                nReVal = mvCameraAcq.SetIntValue("Gain", (int)Gain);
-                if (nReVal != SciCam.SCI_CAMERA_OK)
+                for (int i = 0; i < 3; i++)
                 {
-                    mvCameraAcq.SetFloatValue("Gain", Gain);
+                    nReVal = mvCameraAcq.SetIntValue("Gain", (int)Gain);
+                    if (nReVal != SciCam.SCI_CAMERA_OK)
+                    {
+                        mvCameraAcq.SetFloatValue("Gain", Gain);
+                    }
                 }
                 return true;
             }

TeamAAS-VM/Core/Lights/KCSLightController.cs

@@ -176,6 +176,14 @@ namespace TeamAAS_VP.Core.Lights
                     return brightness;
                 }
             }
+            else if (response.Length == 7)
+            {
+                string valueStr = response.Substring(3, 3);
+                if (int.TryParse(valueStr, out int brightness))
+                {
+                    return brightness;
+                }
+            }
             // 返回默认亮度 0（表示读取失败或设备返回异常）
             return 0;
         }
@@ -192,6 +200,10 @@ namespace TeamAAS_VP.Core.Lights
         {
             string command = $"S{(char)('A' + channelIndex)}0{brightness.ToString("D3")}#";
             var response = await SendCommandAsync(command);
+            if (response=="!")
+            {
+                return true;
+            }
             // 成功时返回通道字母（例如 'A'），去除空白后比较
             return response?.Trim() == ((char)('A' + channelIndex)).ToString();
         }

TeamAAS-VM/Core/Management.cs

@@ -2,6 +2,7 @@
 using Cognex.VisionPro;
 using Cognex.VisionPro.ToolBlock;
 using CSScripting;
+using iTextSharp.testutils;
 using MathNet.Numerics.Distributions;
 using MathNet.Numerics.LinearAlgebra;
 using MathNet.Numerics.RootFinding;
@@ -1838,7 +1839,7 @@ namespace TeamAAS_VP.Core
                                 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 });
 
-                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p4, p3 });
+                                var center = RectangleCenterCalculator.CalculateByDirectMethod(new List<Vector<double>> { p1, p2, p4, p3 });
                                 UpCameraPutResults[0].X = (float)center.Center[0];
                                 UpCameraPutResults[0].Y = (float)center.Center[1];
                                 UpCameraPutResults[0].Z= (float)center.MajorAxisAngle;
@@ -1925,7 +1926,7 @@ namespace TeamAAS_VP.Core
                                 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 });
 
-                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p4, p3 });
+                                var center = RectangleCenterCalculator.CalculateByDirectMethod(new List<Vector<double>> { p1, p2, p4, p3 });
                                 UpCameraPutResults[1].X = (float)center.Center[0];
                                 UpCameraPutResults[1].Y = (float)center.Center[1];
                                 UpCameraPutResults[1].Z = (float)center.MajorAxisAngle;
@@ -1987,15 +1988,15 @@ namespace TeamAAS_VP.Core
 
                             //以LCD的中线创建工具坐标
                             ToolCoord toolCoord = new ToolCoord();
-                            toolCoord.SetTool(UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
+                            toolCoord.ComputeTool(currentPosition.X, currentPosition.Y, currentPosition.U, UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
 
                             
                             //计算放料角度
-                            double angle= currentPosition.U - DowmCameraResults[1].Z + DowmCameraResults[0].Z;
+                            double angle= currentPosition.U - UpCameraPutResults[1].Z + UpCameraPutResults[0].Z;
 
                             //将产品的中心点坐标转换为Tool0下的坐标
                             var point = currentProduct.ScrewPoints[0].Clone();
-                            var pn = Vector<double>.Build.Dense(new double[] { DowmCameraResults[0].X, DowmCameraResults[0].Y });
+                            var pn = Vector<double>.Build.Dense(new double[] { UpCameraPutResults[0].X, UpCameraPutResults[0].Y });
                             var p0 = toolCoord.GetTool0Coord(pn, angle);
                             point.X_Position = (float)p0[0];
                             point.Y_Position = (float)p0[1];

TeamAAS-VM/Core/RectangleCenterCalculator.cs

@@ -169,9 +169,10 @@ namespace TeamAAS_VP.Core
             minorAxis = (minorAxis - majorAxis * minorAxis.DotProduct(majorAxis)).Normalize(2);
 
             // 计算中心点（四个角点的平均）
-            var center = Vector<double>.Build.DenseOfEnumerable(
-                Enumerable.Range(0, 3)
-                    .Select(i => orderedCorners.Select(c => c[i]).Average()));
+            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()));
 
             // 计算长度和宽度
             double length = Distance(topLeft, topRight);
@@ -181,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
             {
@@ -193,7 +194,7 @@ namespace TeamAAS_VP.Core
                 MajorAxisAngle = angle,
                 Corners = orderedCorners,
                 Area = length * width,
-                FitError = fitError
+                FitError = 0//fitError
             };
         }
 

TeamAAS-VM/Services/CameraCalibrationService.cs

@@ -1106,6 +1106,20 @@ namespace TeamAAS_VP.Services
                 {
                     return (false, null, null, null);
                 }
+                //计算相机与机器人坐标系的旋转矩阵
+                var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
+                if (!robotCameraRotationMatrix.IsSuccess)
+                {
+                    return (false, null, null, null);
+                }
+                //自动移动机器人至图像中心点位置
+                var autoMoveResult = await AutoMoveRobotToImageCenter(robot, robotCameraRotationMatrix.RotationMatrix, robotCameraRotationMatrix.PixelScaleX, robotCameraRotationMatrix.PixelScaleY, cancellationToken);
+                if (!autoMoveResult.IsSuccess)
+                {
+                    return (false, null, null, null);
+                }
+                calibrationInfo.CenterPoint.X = (float)autoMoveResult.X;
+                calibrationInfo.CenterPoint.Y = (float)autoMoveResult.Y;
             }
 
             double IntervalX = calibrationInfo.Width / 2;
@@ -1887,6 +1901,8 @@ namespace TeamAAS_VP.Services
             {
                 return (false, null, null);
             }
+
+            //MessageBox.Show("模组移动Mark至相机中心点");
             PointF P11 = new PointF(calibrationInfo.IndependentDownCameraMotionModulePoint.X, calibrationInfo.IndependentDownCameraMotionModulePoint.Y);
             await Task.Delay(300);
 
@@ -1900,6 +1916,7 @@ namespace TeamAAS_VP.Services
             {
                 return (false, null, null);
             }
+            //MessageBox.Show("移动机器人至中心位置");
 
             // 3.计算相机与机器人坐标系的旋转矩阵
             var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
@@ -1910,6 +1927,7 @@ namespace TeamAAS_VP.Services
             pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
             pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
             RobotCameraRotationMatrix = robotCameraRotationMatrix.Item2;
+            //MessageBox.Show("相机与机器人坐标系的旋转矩阵");
 
             // 4.自动移动机器人至图像中心点位置
             var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
@@ -1917,6 +1935,7 @@ namespace TeamAAS_VP.Services
             {
                 return (false, null, null);
             }
+            //MessageBox.Show("移动机器人至图像中心点位置");
             Console.WriteLine($"移动前：{autoMoveResult.X}");
 
             // 5.记录当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
@@ -1941,6 +1960,7 @@ namespace TeamAAS_VP.Services
             }
             PointF P12 = new PointF(movePos.X, movePos.Y);
             await Task.Delay(300);
+            //MessageBox.Show("移动独立模组至标定点位置");
 
             // 7.自动移动机器人至图像中心点位置
             autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
@@ -1948,6 +1968,7 @@ namespace TeamAAS_VP.Services
             {
                 return (false, null, null);
             }
+            //MessageBox.Show("自动移动机器人至图像中心点位置");
 
             // 8.记录当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
             ToolCoord toolCoord1 = new ToolCoord();

TeamAAS-VM/Services/RemoteCommandService.cs

@@ -1463,6 +1463,10 @@ namespace TeamAAS_VP.Services
                 {
                     SendTaskMessage($"相机曝光设置失败！", MessageLevel.Error);
                 }
+                else
+                {
+                    SendTaskMessage($"曝光设置{procedure.ExposureTime}！", MessageLevel.Info);
+                }
                 succed = camera.SetGain(procedure.Gain);
                 if (!succed)
                 {
@@ -1497,7 +1501,7 @@ namespace TeamAAS_VP.Services
                         }
                     }
                 }
-                
+
                 procedure.ToolBlock.Inputs["InputImage"].Value = image;
                 LogHelper.WriteLogInfo($"采集图像完成；用时：{(DateTime.Now - nowtime).Milliseconds}ms");
 
@@ -1591,13 +1595,13 @@ namespace TeamAAS_VP.Services
         /// <param name="outImage"></param>
         /// <param name="outGraphicCollection"></param>
         /// <returns></returns>
-        public bool ExecutePhotoEx(ProcedureModel procedure, Dictionary<string, string> InputTerminal, out CogToolBlockTerminalCollection outputCollection,out ICogImage outImage,out CogGraphicCollection outGraphicCollection)
+        public bool ExecutePhotoEx(ProcedureModel procedure, Dictionary<string, string> InputTerminal, out CogToolBlockTerminalCollection outputCollection, out ICogImage outImage, out CogGraphicCollection outGraphicCollection)
         {
             outImage = null;
             outGraphicCollection = null;
             try
             {
-                
+
                 // 1. 采集图像
                 var camera = _cameraService.GetCamera(procedure.CameraId);
                 bool succed = camera.SetExposureTime(procedure.ExposureTime);
@@ -1677,7 +1681,7 @@ namespace TeamAAS_VP.Services
                         if (item.Value is ICogRecord)
                             record = item.Value as ICogRecord;
                     }
-                    outGraphicCollection= outputCollection["Graphic"].Value as CogGraphicCollection;
+                    outGraphicCollection = outputCollection["Graphic"].Value as CogGraphicCollection;
                     return true;
                 }
                 else
@@ -1710,6 +1714,7 @@ namespace TeamAAS_VP.Services
         {
             DateTime nowtime = DateTime.Now;
             await SetLightBeforePhoto(procedure);
+
             try
             {
                 //拍照失败时，需要多次拍照
@@ -1752,7 +1757,7 @@ namespace TeamAAS_VP.Services
                     double _pixel_x = double.Parse(posParts[0]);
                     double _pixel_y = double.Parse(posParts[1]);
                     double _pixel_u = double.Parse(posParts[2]);
-                    if (calib!=null)
+                    if (calib != null)
                     {
                         //转换点位-像素转换成机器人绝对坐标
                         var calibResult = _calibrationService.ConvertPixelToPosition((_pixel_x, _pixel_y, _pixel_u), robotCoord, calib, RobotBrand.XYZ_Platform);
@@ -1766,7 +1771,7 @@ namespace TeamAAS_VP.Services
                     {
                         return (true, _pixel_x, _pixel_y, _pixel_u);
                     }
-                        
+
                 }
                 SendTaskMessage(Lang.多次拍照失败, MessageLevel.Alarm);
                 return (false, 0, 0, 0);
@@ -1795,8 +1800,8 @@ namespace TeamAAS_VP.Services
         {
             DateTime nowtime = DateTime.Now;
             await SetLightBeforePhoto(procedure);
-            ICogImage outImage=null;
-            CogGraphicCollection outGraphicCollection=null;
+            ICogImage outImage = null;
+            CogGraphicCollection outGraphicCollection = null;
             try
             {
                 //拍照失败时，需要多次拍照

TeamAAS-VM/ViewModels/Product/PlcPointParamsViewModel.cs

@@ -594,6 +594,17 @@ namespace TeamAAS_VP.ViewModels.Product
                 {
                     await Robot.WaitCameraMoveFinishedAsync(point.Number, point);
                 }
+                else if (SelectedIndex == 8)
+                {
+                    //弹窗用户是否确认要示教该点位
+                    if (MessageBox.Show($"确认要示教点位 {SelectedPoint.Number} 吗？", "示教点位", MessageBoxButton.YesNo, MessageBoxImage.Question) != MessageBoxResult.Yes)
+                    {
+                        return;
+                    }
+                    await Robot.AdditionalAxisMoveAsync(Robot.AdditionalAxisList[1].Name, SelectedPoint.Z_Position_Start);
+                    _eventAggregator.GetEvent<SnackbarMessageNotification>().Publish(new MessageParameter() { Msg = $"{Lang.完成}!", Duration = 0.4 });
+                    return;
+                }
                 else if (SelectedIndex == 1)
                 {
                     var curpos = await Robot.GetRobotPosAsync();