独立移动相机刚体变换接口与实现优化

重构刚体变换相关接口，返回旋转和平移矩阵；数据模型拆分齐次矩阵为旋转和平移矩阵；服务与视图模型同步调整，提升校准流程准确性和可维护性；完善注释与偏差提示。

TeamAAS-VM/Interfaces/ICameraCalibrationService.cs

@@ -146,12 +146,12 @@ namespace TeamAAS_VP.Interfaces
         Task<(bool IsSuccess, CalibrationTestResult Result)> ExecuteCalibrationValidation(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken);
 
         /// <summary>
-        /// 独立模组相机计算独立模组步进坐标系与绝对坐标系的刚体变换矩阵
+        /// 独立模组相机计算独立模组步进坐标系与绝对坐标系的刚体变换矩阵(旋转矩阵、平移矩阵)
         /// </summary>
         /// <param name="calibrationInfo"></param>
         /// <param name="robot"></param>
         /// <param name="cancellationToken"></param>
         /// <returns></returns>
-        Task<(bool IsSuccess, double[,] TransformationMatrix)> CalculateIndependentCameraRigidTransformationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
+        Task<(bool IsSuccess, double[,] RotationMatrix, double[,] TranslationMatrix)> CalculateIndependentCameraRigidTransformationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken);
     }
 }

TeamAAS-VM/Models/Calibration/CalibrationInfo.cs

@@ -357,14 +357,24 @@ namespace TeamAAS_VP.Models.Calibration
             set { SetProperty(ref _IndependentDownCameraMotionModuleMoveX, value); }
         }
 
-        private double[,] _IndependentDownCameraMotionModuleHomogeneousMatrix;
+        private double[,] _IndependentDownCameraMotionModuleRotationMatrix;
         /// <summary>
-        /// 独立移动模组的齐次矩阵
+        /// 独立移动模组的旋转矩阵
         /// </summary>
-        public double[,] IndependentDownCameraMotionModuleHomogeneousMatrix
+        public double[,] IndependentDownCameraMotionModuleRotationMatrix
         {
-            get { return _IndependentDownCameraMotionModuleHomogeneousMatrix; }
-            set { SetProperty(ref _IndependentDownCameraMotionModuleHomogeneousMatrix, value); }
+            get { return _IndependentDownCameraMotionModuleRotationMatrix; }
+            set { SetProperty(ref _IndependentDownCameraMotionModuleRotationMatrix, value); }
+        }
+
+        private double[,] _IndependentDownCameraMotionModuleTranslationMatrix;
+        /// <summary>
+        /// 独立移动模组的平移矩阵
+        /// </summary>
+        public double[,] IndependentDownCameraMotionModuleTranslationMatrix
+        {
+            get { return _IndependentDownCameraMotionModuleTranslationMatrix; }
+            set { SetProperty(ref _IndependentDownCameraMotionModuleTranslationMatrix, value); }
         }
 
         private ObservableCollection<RobotPixelPoint> _CalibPoints;

TeamAAS-VM/Services/CameraCalibrationService.cs

@@ -1546,6 +1546,64 @@ namespace TeamAAS_VP.Services
                 //移动机器人至待机位置
                 await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
             }
+            else if (calibrationInfo.CameraMount == Enums.CameraMount.MobileDown_IndependentXYPlatform)
+            {
+                //移动机器人至中心点
+                await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
+                //移动独立模组偏移X距离
+                var movePos = calibrationInfo.IndependentDownCameraMotionModulePoint.Clone();
+                movePos.X += (float)calibrationInfo.IndependentDownCameraMotionModuleMoveX;
+                await ((XYZU_Robot)robot).WaitCameraMoveFinishedAsync(calibrationInfo.CameraMotionModuleIndex, movePos);
+                await Task.Delay(300);
+                //相机拍照获取像素坐标
+                var photoResult = await CaptureAndProcessCallback();
+                if (!photoResult.IsSuccess)
+                {
+                    MessageBox.Show("验证失败：独立模组相机拍照失败或图像处理失败！");
+                    return (false, null);
+                }
+                //将像素坐标转换成机器人坐标系下的坐标
+                PointF pixelpos = new PointF((float)(photoResult.X), (float)(photoResult.Y));
+                Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calibrationInfo.AffineTransformationMaterial);
+                var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelpos.X, pixelpos.Y, 1 });
+                //将此时的坐标转换成独立模组坐标系下的坐标
+                //旋转矩阵
+                Matrix<double> R = Matrix<double>.Build.DenseOfArray(calibrationInfo.IndependentDownCameraMotionModuleRotationMatrix);
+                //平移矩阵
+                Matrix<double> T = Matrix<double>.Build.DenseOfArray(calibrationInfo.IndependentDownCameraMotionModuleTranslationMatrix);
+                //平移向量
+                Vector<double> PT = Vector<double>.Build.Dense(new double[] { T[0, 2], T[1, 2] });
+
+                Vector<double> p100 = Vector<double>.Build.Dense(new double[] { calibrationInfo.IndependentDownCameraMotionModulePoint.X, calibrationInfo.IndependentDownCameraMotionModulePoint.Y, 1 });
+                Vector<double> p101 = Vector<double>.Build.Dense(new double[] { movePos.X, movePos.Y, 1 });
+                //计算独立模组坐标系下的坐标
+                Vector<double> pl1 = R.Inverse() * (rpos - PT);
+                Vector<double> pl2 = pl1 + (p101 - p100);
+                //转换回机器人坐标系下的坐标
+                var finalrpos = R * pl2 + PT;
+
+                //当前Mark的绝对坐标
+                ToolCoord toolCoord1 = new ToolCoord();
+                toolCoord1.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+                double u1 = calibrationInfo.CenterPoint.U;
+                var trans = toolCoord1.GetToolnCoord(Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y }), u1);
+
+                //独立移动模组回到初始位置
+                await ((XYZU_Robot)robot).WaitCameraMoveFinishedAsync(calibrationInfo.CameraMotionModuleIndex, calibrationInfo.IndependentDownCameraMotionModulePoint);
+
+                //计算偏差
+                double dx = finalrpos[0] - trans[0];
+                double dy = finalrpos[1] - trans[1];
+                if (Math.Abs(dx) > 0.05 || Math.Abs(dy) > 0.05)
+                {
+                    MessageBox.Show($"验证失败：独立模组相机标定偏差过大！X方向偏差：{dx:F2} mm，Y方向偏差：{dy:F2} mm");
+                    return (false, null);
+                }
+                else
+                {
+                    MessageBox.Show($"验证成功：独立模组相机标定偏差通过！X方向偏差：{dx:F2} mm，Y方向偏差：{dy:F2} mm");
+                }
+            }
 
             //RMS(均方根值) 标定偏差
             double sumX = 0, sumY = 0;
@@ -1654,20 +1712,20 @@ namespace TeamAAS_VP.Services
         }
 
         /// <summary>
-        /// 独立模组相机计算独立模组步进坐标系与绝对坐标系的刚体变换矩阵
+        /// 独立模组相机计算独立模组步进坐标系与绝对坐标系的刚体变换矩阵(旋转矩阵、平移矩阵)
         /// </summary>
         /// <param name="calibrationInfo"></param>
         /// <param name="robot"></param>
         /// <param name="cancellationToken"></param>
         /// <returns></returns>
-        public async Task<(bool IsSuccess, double[,] TransformationMatrix)> CalculateIndependentCameraRigidTransformationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
+        public async Task<(bool IsSuccess, double[,] RotationMatrix, double[,] TranslationMatrix)> 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);
+                return (false, null, null);
             }
             PointF P11 = new PointF(calibrationInfo.IndependentDownCameraMotionModulePoint.X, calibrationInfo.IndependentDownCameraMotionModulePoint.Y);
 
@@ -1682,7 +1740,7 @@ namespace TeamAAS_VP.Services
             var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
             if (!robotCameraRotationMatrix.IsSuccess)
             {
-                return (false, null);
+                return (false, null, null);
             }
             pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
             pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
@@ -1692,7 +1750,7 @@ namespace TeamAAS_VP.Services
             var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
             if (!autoMoveResult.IsSuccess)
             {
-                return (false, null);
+                return (false, null, null);
             }
 
             // 5.记录当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
@@ -1710,7 +1768,7 @@ namespace TeamAAS_VP.Services
             isFinished = await ((XYZU_Robot)robot).WaitCameraMoveFinishedAsync(calibrationInfo.CameraMotionModuleIndex, movePos);
             if (!isFinished)
             {
-                return (false, null);
+                return (false, null, null);
             }
             PointF P12 = new PointF(movePos.X, movePos.Y);
             await Task.Delay(300);
@@ -1719,7 +1777,7 @@ namespace TeamAAS_VP.Services
             autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
             if (!autoMoveResult.IsSuccess)
             {
-                return (false, null);
+                return (false, null, null);
             }
 
             // 8.记录当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
@@ -1733,22 +1791,22 @@ namespace TeamAAS_VP.Services
             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);
+            ////得到旋转矩阵
+            //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, local.R.ToArray(), local.T.ToArray());
         }
 
         public void Dispose()

TeamAAS-VM/ViewModels/Calibration/CalibIndependentCameraViewModel.cs

@@ -116,6 +116,9 @@ namespace TeamAAS_VP.ViewModels.Calibration
 
         #region 方法
 
+        /// <summary>
+        /// 开始校准
+        /// </summary>
         async void ExecuteStartCalibCommand()
         {
             try
@@ -129,13 +132,14 @@ namespace TeamAAS_VP.ViewModels.Calibration
 
                 IsRunning = false;
                 _cts = new CancellationTokenSource();
-                (bool IsSuccess, double[,] TransformationMatrix) = await _cameraCalibrationService.CalculateIndependentCameraRigidTransformationMatrix(SelectCalibration, Robot, _cts.Token);
+                (bool IsSuccess, double[,] RotationMatrix, double[,] TranslationMatrix) = await _cameraCalibrationService.CalculateIndependentCameraRigidTransformationMatrix(SelectCalibration, Robot, _cts.Token);
                 if (!IsSuccess)
                 {
                     MessageBox.Show("独立移动相机位置校准失败", $"{Lang.自动校准}", MessageBoxButton.OK, MessageBoxImage.Warning);
                     return;
                 }
-                SelectCalibration.IndependentDownCameraMotionModuleHomogeneousMatrix = TransformationMatrix;
+                SelectCalibration.IndependentDownCameraMotionModuleRotationMatrix = RotationMatrix;
+                SelectCalibration.IndependentDownCameraMotionModuleTranslationMatrix = TranslationMatrix;
                 NavigationParameters param = new NavigationParameters();
                 param.Add("SelectCalibration", SelectCalibration);
                 _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationAuto", param);