九点标定与验证流程完善，接口与健壮性提升

完善了CameraCalibrationService的九点自动标定流程，AutoNinePointCalibration接口返回九点数组，支持更丰富的数据处理。新增ExecuteCalibrationValidation方法，实现标定结果验证。多处增加参数校验、机器人连接判断和取消支持，提升健壮性。优化数据结构和注释，增强可维护性和扩展性。

TeamAAS-VM/Interfaces/ICameraCalibrationService.cs

@@ -133,7 +133,7 @@ namespace TeamAAS_VP.Interfaces
         /// NinePoint - 九点标定结果封装（像素点与对应的机器人位姿）；
         /// Result - 详细的标定结果与统计信息（误差、残差等）。
         /// </returns>
-        Task<(bool IsSuccess, Matrix<double> AffineTransformationMaterial, RobotPixelPoint NinePoint, CalibrationResult Result)> AutoNinePointCalibration(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken);
+        Task<(bool IsSuccess, Matrix<double> AffineTransformationMaterial, RobotPixelPoint[] NinePoint, CalibrationResult Result)> AutoNinePointCalibration(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken);
 
         /// <summary>
         /// 执行校准验证。

TeamAAS-VM/Services/CameraCalibrationService.cs

@@ -2,12 +2,16 @@ using MaterialDesignThemes.Wpf;
 using MathNet.Numerics;
 using MathNet.Numerics.LinearAlgebra;
 using OpenCvSharp;
+using Prism.Regions;
 using Prism.Services.Dialogs;
 using System;
+using System.Collections.Generic;
 using System.Drawing;
+using System.Linq;
 using System.Security.Cryptography;
 using System.Threading;
 using System.Threading.Tasks;
+using System.Windows;
 using TeamAAS_VP.Controls;
 using TeamAAS_VP.Core;
 using TeamAAS_VP.Interfaces;
@@ -757,9 +761,421 @@ namespace TeamAAS_VP.Services
         /// NinePoint - 九点标定结果封装（像素点与对应的机器人位姿）；
         /// Result - 详细的标定结果与统计信息（误差、残差等）。
         /// </returns>
-        public async Task<(bool IsSuccess, Matrix<double> AffineTransformationMaterial, RobotPixelPoint NinePoint, CalibrationResult Result)> AutoNinePointCalibration(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
+        public async Task<(bool IsSuccess, Matrix<double> AffineTransformationMaterial, RobotPixelPoint[] NinePoint, CalibrationResult Result)> AutoNinePointCalibration(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
         {
+            if (calibrationInfo == null) return (false, null, null, null);
+            if (robot == null || !robot.IsConnected) return (false, null, null, null);
+
+            double IntervalX = calibrationInfo.Width / 2;
+            double IntervalY = calibrationInfo.Height / 2;
+            Vector<double> P1 = Vector<double>.Build.Dense(new double[] { -IntervalX, -IntervalY });
+            Vector<double> P2 = Vector<double>.Build.Dense(new double[] { 0, -IntervalY });
+            Vector<double> P3 = Vector<double>.Build.Dense(new double[] { IntervalX, -IntervalY });
+            Vector<double> P4 = Vector<double>.Build.Dense(new double[] { IntervalX, 0 });
+            Vector<double> P5 = Vector<double>.Build.Dense(new double[] { 0, 0 });
+            Vector<double> P6 = Vector<double>.Build.Dense(new double[] { -IntervalX, 0 });
+            Vector<double> P7 = Vector<double>.Build.Dense(new double[] { -IntervalX, IntervalY });
+            Vector<double> P8 = Vector<double>.Build.Dense(new double[] { 0, IntervalY });
+            Vector<double> P9 = Vector<double>.Build.Dense(new double[] { IntervalX, IntervalY });
+            //发送校准的参数
+            await robot.CalibParameAsync(calibrationInfo.Pick, calibrationInfo.Speed, calibrationInfo.Accel, calibrationInfo.Power, calibrationInfo.WaitSuction, calibrationInfo.WaitBlow);
+
+            var P0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
+            if (robot.Brand == Enums.RobotBrand.XYZ_Platform)
+            {
+
+                //移动机器人至中心位置
+                await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
+                if (cancellationToken.IsCancellationRequested)
+                {
+                    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;
+                calibrationInfo.MarkPoint = calibrationInfo.CenterPoint.Clone();
+            }
+
+            /// <summary>
+            /// 机器人与相机之间的旋转矩阵
+            /// </summary>
+            Matrix<double> RobotCameraRotationMatrix = Matrix<double>.Build.DenseOfArray(calibrationInfo.RobotCameraRotationMatrix);
+            //1
+            P1 = RobotCameraRotationMatrix.Inverse() * P1 + P0;
+            //2
+            P2 = RobotCameraRotationMatrix.Inverse() * P2 + P0;
+            //3
+            P3 = RobotCameraRotationMatrix.Inverse() * P3 + P0;
+            //4
+            P4 = RobotCameraRotationMatrix.Inverse() * P4 + P0;
+            //5
+            P5 = RobotCameraRotationMatrix.Inverse() * P5 + P0;
+            //6
+            P6 = RobotCameraRotationMatrix.Inverse() * P6 + P0;
+            //7
+            P7 = RobotCameraRotationMatrix.Inverse() * P7 + P0;
+            //8
+            P8 = RobotCameraRotationMatrix.Inverse() * P8 + P0;
+            //9
+            P9 = RobotCameraRotationMatrix.Inverse() * P9 + P0;
+
+            List<RPoint> rPoints = new List<RPoint>();
+            rPoints.Add(new RPoint()
+            {
+                Number = 1,
+                X = (float)P1[0],
+                Y = (float)P1[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 2,
+                X = (float)P2[0],
+                Y = (float)P2[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 3,
+                X = (float)P3[0],
+                Y = (float)P3[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 4,
+                X = (float)P4[0],
+                Y = (float)P4[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 5,
+                X = (float)P5[0],
+                Y = (float)P5[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 6,
+                X = (float)P6[0],
+                Y = (float)P6[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 7,
+                X = (float)P7[0],
+                Y = (float)P7[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 8,
+                X = (float)P8[0],
+                Y = (float)P8[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 9,
+                X = (float)P9[0],
+                Y = (float)P9[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            List<PointF> PixelPoslist = new List<PointF>();
+            if (calibrationInfo.CalibPoints == null)
+            {
+                calibrationInfo.CalibPoints = new System.Collections.ObjectModel.ObservableCollection<RobotPixelPoint>();
+            }
+            else
+            {
+                calibrationInfo.CalibPoints.Clear();
+            }
 
+            //移动相机标定时，需要先记住特征点的绝对坐标（相对于机器人坐标系） P0
+            if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
+            {
+                //移动机器人至中心位置
+                await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
+                if (cancellationToken.IsCancellationRequested)
+                {
+                    return (false, null, null, null);
+                }
+
+                //将当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
+                ToolCoord toolCoord = new ToolCoord();
+                toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+                Vector<double> p = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y }); ;
+                var pn = toolCoord.GetToolnCoord(p, calibrationInfo.CenterPoint.U);
+                if (calibrationInfo.MarkPoint == null)
+                {
+                    calibrationInfo.MarkPoint = new RPoint();
+                }
+                calibrationInfo.MarkPoint = calibrationInfo.CenterPoint.Clone();
+                calibrationInfo.MarkPoint.X = (float)pn[0];
+                calibrationInfo.MarkPoint.Y = (float)pn[1];
+            }
+
+
+            for (int i = 0; i < rPoints.Count; i++)
+            {
+                //移动机器人至i
+                if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
+                {
+                    await robot.CalibMotionAsync(rPoints[i], 0);
+                    if (cancellationToken.IsCancellationRequested)
+                    {
+                        return (false, null, null, null);
+                    }
+                    //关闭吸气
+                    await robot.CalibOutIOAsync(false);
+                    //移动机器人至待机位置
+                    await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
+                }
+                else
+                {
+                    await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
+                }
+                if (cancellationToken.IsCancellationRequested)
+                {
+                    return (false, null, null, null);
+                }
+
+                //相机拍照并处理图像
+                var photoResult = await CaptureAndProcessCallback();
+                if (!photoResult.IsSuccess)
+                {
+                    return (false, null, null, null);
+                }
+                PointF curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));
+                PixelPoslist.Add(curPixel);
+
+                if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
+                {
+                    //移动机器人至i
+                    await robot.CalibMotionAsync(rPoints[i], 0);
+                    if (cancellationToken.IsCancellationRequested)
+                    {
+                        return (false, null, null, null);
+                    }
+                    //打开吸气
+                    await robot.CalibOutIOAsync(true);
+                }
+
+                calibrationInfo.CalibPoints.Add(new RobotPixelPoint()
+                {
+                    Number = i + 1,
+                    Robot = new System.Drawing.PointF(rPoints[i].X, rPoints[i].Y),
+                    Pixel = new System.Drawing.PointF(PixelPoslist[i].X, PixelPoslist[i].Y)
+                });
+                if (cancellationToken.IsCancellationRequested)
+                {
+                    return (false, null, null, null);
+                }
+            }
+            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
+            {
+                //移动机器人至待机位置
+                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
+            }
+
+            if (cancellationToken.IsCancellationRequested)
+            {
+                return (false, null, null, null);
+            }
+            Mat matPixel = new Mat(9, 2, MatType.CV_64F);
+            Mat matRobot = new Mat(9, 2, MatType.CV_64F);
+
+            for (int i = 0; i < 9; i++)
+            {
+                matPixel.Set<double>(i, 0, PixelPoslist[i].X);
+                matPixel.Set<double>(i, 1, PixelPoslist[i].Y);
+                //如果是移动相机拍照需要将机器人的9点进行一个变换，为了达到Mark点动，相机不动的目的
+                if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
+                {
+                    //计算每个点相对于第一个点移动的偏移量，将这个偏移量
+                    double ox = rPoints[i].X - rPoints[0].X;
+                    double oy = rPoints[i].Y - rPoints[0].Y;
+                    matRobot.Set<double>(i, 0, calibrationInfo.MarkPoint.X - ox);
+                    matRobot.Set<double>(i, 1, calibrationInfo.MarkPoint.Y - oy);
+                }
+                else if (calibrationInfo.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
+                {
+                    //计算每个点相对于第一个点移动的偏移量，将这个偏移量
+                    double ox = rPoints[i].X - rPoints[0].X;
+                    double oy = rPoints[i].Y - rPoints[0].Y;
+                    matRobot.Set<double>(i, 0, calibrationInfo.MarkPoint.X - ox);
+                    matRobot.Set<double>(i, 1, calibrationInfo.MarkPoint.Y - oy);
+                }
+                else
+                {
+                    //将tool 0下的坐标转换成Tool n下的坐标
+                    if (calibrationInfo.Tool != null)
+                    {
+                        ToolCoord toolCoord = new ToolCoord();
+                        toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+                        double u1 = rPoints[i].U;
+                        if (robot.Brand == Enums.RobotBrand.Schneider)
+                        {
+                            u1 *= -1;
+                        }
+                        var trans = toolCoord.GetToolnCoord(Vector<double>.Build.Dense(new double[] { rPoints[i].X, rPoints[i].Y }), u1);
+                        matRobot.Set<double>(i, 0, trans[0]);
+                        matRobot.Set<double>(i, 1, trans[1]);
+                    }
+                    else
+                    {
+                        matRobot.Set<double>(i, 0, rPoints[i].X);
+                        matRobot.Set<double>(i, 1, rPoints[i].Y);
+                    }
+                }
+
+            }
+            Mat mat2d = Cv2.EstimateAffine2D(matPixel, matRobot);
+            double[,] vv = { { mat2d.Get<double>(0, 0), mat2d.Get<double>(0, 1), mat2d.Get<double>(0, 2) }, { mat2d.Get<double>(1, 0), mat2d.Get<double>(1, 1), mat2d.Get<double>(1, 2) } };
+            Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(vv);
+            calibrationInfo.AffineTransformationMaterial = matrix.ToArray();
+
+
+            //mat2d.Get<double>(0, 0):表示x方向上的缩放。大于 1，则表示进行了放大操作；小于 1，则表示进行了缩小操作；等于 1，则表示没有进行缩放操作。
+            //mat2d.Get<double>(0, 1):表示垂直错切参数，与M[1,0]一起用于计算旋转角度
+            //mat2d.Get<double>(0, 2):表示x方向上的平移。
+            //mat2d.Get<double>(1, 0):表示水平错切参数，与M[0,1]一起用于计算旋转角度。
+            //mat2d.Get<double>(1, 1):表示y方向上的缩放。大于 1，则表示进行了放大操作；小于 1，则表示进行了缩小操作；等于 1，则表示没有进行缩放操作。
+            //mat2d.Get<double>(1, 2):表示y方向上的平移。
+            //double RotationAngle= Math.Atan2(mat2d.Get<double>(1, 0), mat2d.Get<double>(0, 1))*180/Math.PI; //旋转角度
+            //RMS(均方根值) 标定偏差
+            double sumX = 0, sumY = 0;
+            List<double> differenceX = new List<double>();
+            List<double> differenceY = new List<double>();
+            for (int i = 0; i < 9; i++)
+            {
+                var pixel = Vector<double>.Build.Dense(new double[] { PixelPoslist[i].X, PixelPoslist[i].Y, 1 });
+                if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
+                {
+                    //计算每个点相对于第一个点移动的偏移量，Mark点减去这个偏移量及是图像中对应的实际坐标
+                    double ox = rPoints[i].X - rPoints[0].X;
+                    double oy = rPoints[i].Y - rPoints[0].Y;
+                    differenceX.Add((calibrationInfo.MarkPoint.X - ox) - (matrix * pixel)[0]);
+                    differenceY.Add((calibrationInfo.MarkPoint.Y - oy) - (matrix * pixel)[1]);
+                }
+                else if (calibrationInfo.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
+                {
+                    double ox = rPoints[i].X - rPoints[0].X;
+                    double oy = rPoints[i].Y - rPoints[0].Y;
+                    differenceX.Add((calibrationInfo.MarkPoint.X - ox) - (matrix * pixel)[0]);
+                    differenceY.Add((calibrationInfo.MarkPoint.Y - oy) - (matrix * pixel)[1]);
+                }
+                else
+                {
+                    //将tool 0下的坐标转换成Tool n下的坐标
+                    if (calibrationInfo.Tool != null)
+                    {
+                        ToolCoord toolCoord = new ToolCoord();
+                        toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+                        double u1 = rPoints[i].U;
+                        if (robot.Brand == Enums.RobotBrand.Schneider)
+                        {
+                            u1 *= -1;
+                        }
+                        var trans = toolCoord.GetToolnCoord(Vector<double>.Build.Dense(new double[] { rPoints[i].X, rPoints[i].Y }), u1);
+                        differenceX.Add(trans[0] - (matrix * pixel)[0]);
+                        differenceY.Add(trans[1] - (matrix * pixel)[1]);
+                    }
+                    else
+                    {
+                        differenceX.Add(rPoints[i].X - (matrix * pixel)[0]);
+                        differenceY.Add(rPoints[i].Y - (matrix * pixel)[1]);
+                    }
+                }
+
+
+                sumX += Math.Pow(differenceX[differenceX.Count - 1], 2);
+                sumY += Math.Pow(differenceY[differenceY.Count - 1], 2);
+            }
+            double rmsX, rmsY;
+            rmsX = Math.Sqrt(sumX / 9);
+            rmsY = Math.Sqrt(sumY / 9);
+
+            CalibrationResult calibrationResult = new CalibrationResult();
+            calibrationResult.ScaleX = mat2d.Get<double>(0, 0);
+            calibrationResult.ScaleY = mat2d.Get<double>(1, 1);
+            calibrationResult.TranslationX = mat2d.Get<double>(0, 2);
+            calibrationResult.TranslationY = mat2d.Get<double>(1, 2);
+            calibrationResult.Rotate = Math.Atan2(mat2d.Get<double>(1, 0), mat2d.Get<double>(0, 1)) * 180 / Math.PI;
+            calibrationResult.RMSX = rmsX;
+            calibrationResult.RMSY = rmsY;
+            calibrationResult.MaxErrorValueX = differenceX.Max();
+            calibrationResult.MaxErrorValueY = differenceY.Max();
+            calibrationInfo.CalibrationResult = calibrationResult;
+
+            return (true, matrix, calibrationInfo.CalibPoints.ToArray(), calibrationResult);
         }
 
         /// <summary>
@@ -772,7 +1188,235 @@ namespace TeamAAS_VP.Services
         /// <returns>元组字段：IsSuccess 表示验证是否通过；Result 包含详细的验证统计与误差分析。</returns>
         public async Task<(bool IsSuccess, CalibrationTestResult Result)> ExecuteCalibrationValidation(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
         {
+            if (calibrationInfo == null) return (false, null);
+            if (robot == null || !robot.IsConnected) return (false, null);
+
+            double IntervalX = calibrationInfo.Width * 0.90 / 2;
+            double IntervalY = calibrationInfo.Height * 0.90 / 2;
+            Vector<double> P1 = Vector<double>.Build.Dense(new double[] { -IntervalX, -IntervalY });
+            Vector<double> P2 = Vector<double>.Build.Dense(new double[] { IntervalX, -IntervalY });
+            Vector<double> P3 = Vector<double>.Build.Dense(new double[] { 0, 0 });
+            Vector<double> P4 = Vector<double>.Build.Dense(new double[] { -IntervalX, IntervalY });
+            Vector<double> P5 = Vector<double>.Build.Dense(new double[] { IntervalX, IntervalY });
+
+            var P0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
+            /// <summary>
+            /// 机器人与相机之间的旋转矩阵
+            /// </summary>
+            Matrix<double> RobotCameraRotationMatrix = Matrix<double>.Build.DenseOfArray(calibrationInfo.RobotCameraRotationMatrix);
+            //1
+            P1 = RobotCameraRotationMatrix.Inverse() * P1 + P0;
+            //2
+            P2 = RobotCameraRotationMatrix.Inverse() * P2 + P0;
+            //3
+            P3 = RobotCameraRotationMatrix.Inverse() * P3 + P0;
+            //4
+            P4 = RobotCameraRotationMatrix.Inverse() * P4 + P0;
+            //5
+            P5 = RobotCameraRotationMatrix.Inverse() * P5 + P0;
+            List<RPoint> rPoints = new List<RPoint>();
+            rPoints.Add(new RPoint()
+            {
+                Number = 1,
+                X = (float)P1[0],
+                Y = (float)P1[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 2,
+                X = (float)P2[0],
+                Y = (float)P2[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 3,
+                X = (float)P3[0],
+                Y = (float)P3[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 4,
+                X = (float)P4[0],
+                Y = (float)P4[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+            rPoints.Add(new RPoint()
+            {
+                Number = 5,
+                X = (float)P5[0],
+                Y = (float)P5[1],
+                Z = calibrationInfo.CenterPoint.Z,
+                U = calibrationInfo.CenterPoint.U,
+                V = calibrationInfo.CenterPoint.V,
+                W = calibrationInfo.CenterPoint.W,
+                Hand = calibrationInfo.CenterPoint.Hand,
+                Local = calibrationInfo.CenterPoint.Local,
+                Tool = calibrationInfo.CenterPoint.Tool
+            });
+
+            List<Vector<double>> TestPos = new List<Vector<double>>();
+            for (int i = 0; i < rPoints.Count; i++)
+            {
+                if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
+                {
+                    //移动机器人至i
+                    await robot.CalibMotionAsync(rPoints[i], 0);
+                    if (cancellationToken.IsCancellationRequested)
+                    {
+                        return (false, null);
+                    }
+                    //关闭吸气
+                    await robot.CalibOutIOAsync(false);
+                    //移动机器人至待机位置
+                    await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
+                }
+                else
+                {
+                    //移动机器人至i
+                    await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
+                }
+                if (cancellationToken.IsCancellationRequested)
+                {
+                    return (false, null);
+                }
 
+                //相机拍照并处理图像
+                var photoResult = await CaptureAndProcessCallback();
+                if (!photoResult.IsSuccess)
+                {
+                    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 });
+                TestPos.Add(rpos);
+                if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
+                {
+                    //移动机器人至i
+                    await robot.CalibMotionAsync(rPoints[i], 0);
+                    //打开吸气
+                    await robot.CalibOutIOAsync(true);
+                }
+
+                if (cancellationToken.IsCancellationRequested)
+                {
+                    return (false, null);
+                }
+            }
+
+            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
+            {
+                //移动机器人至待机位置
+                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
+            }
+
+            //RMS(均方根值) 标定偏差
+            double sumX = 0, sumY = 0;
+            List<double> differenceX = new List<double>();
+            List<double> differenceY = new List<double>();
+            for (int i = 0; i < 5; i++)
+            {
+                if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
+                {
+                    //像素直接转换成mm时的点位
+                    double Robot_X, Robot_Y;
+                    Robot_X = TestPos[i][0];
+                    Robot_Y = TestPos[i][1];
+
+                    //机器人当前TOOL 0下的坐标
+                    double curpos_x = 0, curpos_y = 0, curpos_u = 0;
+                    curpos_x = rPoints[i].X;
+                    curpos_y = rPoints[i].Y;
+                    curpos_u = rPoints[i].U;
+
+                    //得到旋转矩阵
+                    double angle = Math.PI * (curpos_u - calibrationInfo.MarkPoint.U) / 180;
+                    // 创建T矩阵
+                    Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
+                    {
+                                { Math.Cos(angle), -Math.Sin(angle) },
+                                { Math.Sin(angle), Math.Cos(angle) }
+                    });
+
+                    Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calibrationInfo.CalibPoints[0].Robot.X, Robot_Y - calibrationInfo.CalibPoints[0].Robot.Y });
+                    Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });
+
+                    var cc = rotationMatrix * p3 + phere;
+
+                    differenceX.Add(cc[0] - calibrationInfo.MarkPoint.X);
+                    differenceY.Add(cc[1] - calibrationInfo.MarkPoint.Y);
+                }
+                else
+                {
+                    //将tool 0下的坐标转换成Tool n下的坐标
+                    if (calibrationInfo.Tool != null)
+                    {
+                        ToolCoord toolCoord = new ToolCoord();
+                        toolCoord.SetTool(calibrationInfo.Tool.X, calibrationInfo.Tool.Y);
+                        double u1 = rPoints[i].U;
+                        if (robot.Brand == Enums.RobotBrand.Schneider)
+                        {
+                            u1 *= -1;
+                        }
+                        var trans = toolCoord.GetToolnCoord(Vector<double>.Build.Dense(new double[] { rPoints[i].X, rPoints[i].Y }), u1);
+                        differenceX.Add(trans[0] - TestPos[i][0]);
+                        differenceY.Add(trans[1] - TestPos[i][1]);
+                    }
+                    else
+                    {
+                        differenceX.Add(rPoints[i].X - TestPos[i][0]);
+                        differenceY.Add(rPoints[i].Y - TestPos[i][1]);
+                    }
+                }
+
+                sumX += Math.Pow(differenceX[differenceX.Count - 1], 2);
+                sumY += Math.Pow(differenceY[differenceY.Count - 1], 2);
+            }
+            double rmsX, rmsY;
+            rmsX = Math.Sqrt(sumX / 5);
+            rmsY = Math.Sqrt(sumY / 5);
+
+            CalibrationTestResult calibrationTestResult = new CalibrationTestResult();
+            calibrationTestResult.MaxErrorValueX = differenceX.Max();
+            calibrationTestResult.MinErrorValueX = differenceX.Min();
+            calibrationTestResult.MaxErrorValueY = differenceY.Max();
+            calibrationTestResult.MinErrorValueY = differenceY.Min();
+            calibrationTestResult.RMSEX = rmsX;
+            calibrationTestResult.RMSEY = rmsY;
+            return (true, calibrationTestResult);
+        }
+
+        public void Dispose()
+        {
+            GC.SuppressFinalize(this);
         }
     }
 }