新增独立模组X轴移动参数及异步刚性变换计算

在 CalibrationInfo.cs 中新增 IndependentDownCameraMotionModuleMoveX 属性，用于记录独立模组X轴移动距离。重构 CameraCalibrationService.cs 中的 CalculateIndependentCameraRigidTransformationMatrix 方法为异步实现，完善了机器人与相机坐标系刚性变换的计算流程，增加了异常处理和流程控制，提升了健壮性和可维护性。

TeamAAS-VM/Models/Calibration/CalibrationInfo.cs

@@ -347,6 +347,16 @@ namespace TeamAAS_VP.Models.Calibration
             set { SetProperty(ref _IndependentDownCameraMotionModulePoint, value); }
         }
 
+        private double _IndependentDownCameraMotionModuleMoveX;
+        /// <summary>
+        /// 独立模组X轴移动的距离
+        /// </summary>
+        public double IndependentDownCameraMotionModuleMoveX
+        {
+            get { return _IndependentDownCameraMotionModuleMoveX; }
+            set { SetProperty(ref _IndependentDownCameraMotionModuleMoveX, value); }
+        }
+
         private ObservableCollection<RobotPixelPoint> _CalibPoints;
 
         /// <summary>

TeamAAS-VM/Services/CameraCalibrationService.cs

@@ -28,7 +28,6 @@ namespace TeamAAS_VP.Services
     /// </summary>
     public class CameraCalibrationService : ICameraCalibrationService
     {
-
         /// <summary>
         /// 触发相机拍照并进行图像处理的回调函数
         /// 在标定过程中需要获取图像特征点时调用此回调
@@ -1657,13 +1656,99 @@ namespace TeamAAS_VP.Services
         /// <summary>
         /// 独立模组相机计算独立模组步进坐标系与绝对坐标系的刚体变换矩阵
         /// </summary>
-        /// <param name="markPoint"></param>
-        /// <param name="calibPoints"></param>
+        /// <param name="calibrationInfo"></param>
+        /// <param name="robot"></param>
+        /// <param name="cancellationToken"></param>
         /// <returns></returns>
-        public (bool IsSuccess, double[,] TransformationMatrix) CalculateIndependentCameraRigidTransformationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
+        public async Task<(bool IsSuccess, double[,] TransformationMatrix)> CalculateIndependentCameraRigidTransformationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
         {
             //模组移动Mark至相机中心点，此时Mark的绝对坐标是:P0,步进坐标:P1
+            // 1.移动独立模组至待机点
+            bool isFinished = await ((XYZU_Robot)robot).WaitCameraMoveFinishedAsync(calibrationInfo.CameraMotionModuleIndex, calibrationInfo.IndependentDownCameraMotionModulePoint);
+            if (!isFinished)
+            {
+                return (false, null);
+            }
+            PointF P11 = new PointF(calibrationInfo.IndependentDownCameraMotionModulePoint.X, calibrationInfo.IndependentDownCameraMotionModulePoint.Y);
+
+            double pixelScaleX = 0;
+            double pixelScaleY = 0;
+            Matrix<double> RobotCameraRotationMatrix = null;
+
+            // 2.移动机器人至中心位置
+            await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
 
+            // 3.计算相机与机器人坐标系的旋转矩阵
+            var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
+            if (!robotCameraRotationMatrix.IsSuccess)
+            {
+                return (false, null);
+            }
+            pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
+            pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
+            RobotCameraRotationMatrix = robotCameraRotationMatrix.Item2;
+
+            // 4.自动移动机器人至图像中心点位置
+            var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
+            if (!autoMoveResult.IsSuccess)
+            {
+                return (false, null);
+            }
+
+            // 5.记录当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
+            ToolCoord toolCoord = new ToolCoord();
+            toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+            //获取机器人当前坐标
+            var currentRobotPos = await robot.GetRobotPosAsync();
+            Vector<double> p = Vector<double>.Build.Dense(new double[] { currentRobotPos.X, currentRobotPos.Y }); ;
+            var p1 = toolCoord.GetToolnCoord(p, currentRobotPos.U);
+            PointF P1 = new PointF((float)p1[0], (float)p1[1]);
+
+            // 6.移动独立模组至标定点位置
+            var movePos = calibrationInfo.IndependentDownCameraMotionModulePoint.Clone();
+            movePos.X += (float)calibrationInfo.IndependentDownCameraMotionModuleMoveX;
+            isFinished = await ((XYZU_Robot)robot).WaitCameraMoveFinishedAsync(calibrationInfo.CameraMotionModuleIndex, movePos);
+            if (!isFinished)
+            {
+                return (false, null);
+            }
+            PointF P12 = new PointF(movePos.X, movePos.Y);
+            await Task.Delay(300);
+
+            // 7.自动移动机器人至图像中心点位置
+            autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
+            if (!autoMoveResult.IsSuccess)
+            {
+                return (false, null);
+            }
+
+            // 8.记录当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
+            ToolCoord toolCoord1 = new ToolCoord();
+            toolCoord1.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+            //获取机器人当前坐标
+            currentRobotPos = await robot.GetRobotPosAsync();
+            Vector<double> pf = Vector<double>.Build.Dense(new double[] { currentRobotPos.X, currentRobotPos.Y }); ;
+            var p2 = toolCoord1.GetToolnCoord(pf, currentRobotPos.U);
+            PointF P2 = new PointF((float)p2[0], (float)p2[1]);
+            CoordinateTransformer local = new CoordinateTransformer(P1, P2, P11, P12);
+
+            // 9.计算刚体变换矩阵
+            //得到旋转矩阵
+            var rigidMatrix = local.InvR;
+            //平移矩阵
+            var translation = local.T;
+            double[,] TransformationMatrix = new double[3, 3];
+            TransformationMatrix[0, 0] = rigidMatrix[0, 0];
+            TransformationMatrix[0, 1] = rigidMatrix[0, 1];
+            TransformationMatrix[0, 2] = translation[0, 0];
+            TransformationMatrix[1, 0] = rigidMatrix[1, 0];
+            TransformationMatrix[1, 1] = rigidMatrix[1, 1];
+            TransformationMatrix[1, 2] = translation[1, 0];
+            TransformationMatrix[2, 0] = 0;
+            TransformationMatrix[2, 1] = 0;
+            TransformationMatrix[2, 2] = 1;
+
+            return (true, TransformationMatrix);
         }
 
         public void Dispose()