Display_D_HP/TeamAAS-VM/Services/CameraCalibrationService.cs

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;
using TeamAAS_VP.Models;
using TeamAAS_VP.Models.Calibration;
using TeamAAS_VP.Models.Robot;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.Services
{
    /// <summary>
    /// 相机标定服务的骨架实现。实际的标定算法和数据持久化由后续实现补充。
    /// </summary>
    public class CameraCalibrationService : ICameraCalibrationService
    {

        /// <summary>
        /// 触发相机拍照并进行图像处理的回调函数
        /// 在标定过程中需要获取图像特征点时调用此回调
        /// Tuple 字段含义：
        /// Item1 - IsSuccess: 拍照及图像处理是否成功；
        /// Item2 - X: 图像处理后得到的机器人目标 X（mm）；
        /// Item3 - Y: 图像处理后得到的机器人目标 Y（mm）；
        /// Item4 - U: 图像处理后得到的机器人目标 U（工具朝向）。
        /// </summary>
        public Func<Task<(bool IsSuccess, double X, double Y, double U, int ImageWidth, int ImageHeight)>> CaptureAndProcessCallback { get; set; }

        /// <summary>
        /// 固定向下相机校准工具坐标。
        /// 该方法用于在相机固定、朝下安装且工具不随 J4 移动的情况下标定相机与机器人工具坐标系之间的关系。
        /// </summary>
        /// <param name="calibrationInfo">包含标定时所需的相机参数、图像处理配置与标定点信息的对象。</param>
        /// <param name="robot">实现了 <see cref="IRobot"/> 的机器人抽象，用于获取/移动机器人位姿。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>
        /// 一个包含以下字段的元组：
        /// IsSuccess - 标定是否成功；
        /// Matrix<double> - 图像与像素坐标系之间的旋转（或仿射）矩阵（行列顺序与具体实现约定）；
        /// ToolX - 标定得到的工具坐标系在机器人基坐标系下的 X（单位与机器人一致）；
        /// ToolY - 标定得到的工具坐标系在机器人基坐标系下的 Y（单位与机器人一致）；
        /// PixelScaleX - 像素到物理距离在 X 方向的缩放因子（如 mm/px）；
        /// PixelScaleY - 像素到物理距离在 Y 方向的缩放因子（如 mm/px）。
        /// </returns>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateFixedDownCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            //移动机器人至待机位置
            await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0, 0, 0);
            }

            if (calibrationInfo.Pick.PickPlaceModel == Enums.PickPlaceModel.Inhal)
            {
                //打开吸气
                await robot.CalibOutIOAsync(true);
            }
            else
            {
                //张开夹爪
                //夹爪张开为false,夹爪闭合为True
                await robot.CalibOutIOAsync(false);
            }
            var view = new ShowMessage(Lang.提示, Lang.请将校准块放置在吸嘴中心);
            var result = await DialogHost.Show(view, "CalibToolDialog", null, null, null);
            if (!((bool)result)) return (false, null, 0, 0, 0, 0);
            if (calibrationInfo.Pick.PickPlaceModel == Enums.PickPlaceModel.Clamp)
            {
                //夹紧夹爪
                await robot.CalibOutIOAsync(true);
            }

            //计算相机与机器人坐标系的旋转矩阵
            var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
            if (!robotCameraRotationMatrix.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
            pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
            RobotCameraRotationMatrix = robotCameraRotationMatrix.Item2;
            //相机拍照并处理图像
            var photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            PointF P0 = new PointF((float)(photoResult.X), (float)(photoResult.Y));

            RPoint RobotU = calibrationInfo.CenterPoint.Clone();
            RobotU.U += (float)calibrationInfo.Angle;
            //移动机器人中心点
            await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0, 0, 0);
            }
            //打开吸气
            await robot.CalibOutIOAsync(true);

            //机器人U轴旋转
            await robot.CalibMotionAsync(RobotU, 0);
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0, 0, 0);
            }

            //关闭吸气
            await robot.CalibOutIOAsync(false);
            //移动机器人至待机位置
            await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0, 0, 0);
            }

            //相机拍照并处理图像
            photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            PointF curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));

            bool IsFinish = false;
            for (int i = 0; i < 10; i++)
            {
                //计算需要移动的偏移量，目的是逼近中心点
                double offsetx = (P0.X - curPixel.X) * pixelScaleX;
                double offsety = (P0.Y - curPixel.Y) * pixelScaleY;
                var pos = RobotCameraRotationMatrix.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });
                //移动机器人取标定块
                await robot.CalibMotionAsync(RobotU, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0, 0, 0);
                }
                //打开吸气
                await robot.CalibOutIOAsync(true);

                RobotU.X += (float)pos[0];
                RobotU.Y += (float)pos[1];
                //机器人移动
                await robot.CalibMotionAsync(RobotU, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0, 0, 0);
                }

                //关闭吸气
                await robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0, 0, 0);
                }

                photoResult = await CaptureAndProcessCallback();
                if (!photoResult.IsSuccess)
                {
                    return (false, null, 0, 0, 0, 0);
                }
                curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));
                offsetx = P0.X - curPixel.X;
                offsety = P0.Y - curPixel.Y;
                //await Console.Out.WriteLineAsync($"移动后的差异：{offsetx}，{offsety}");
                if (Math.Abs(offsetx) < 1 && Math.Abs(offsetx) < 1)
                {
                    //移动机器人取标定块
                    await robot.CalibMotionAsync(RobotU, 0);
                    if (cancellationToken.IsCancellationRequested)
                    {
                        return (false, null, 0, 0, 0, 0);
                    }
                    //打开吸气
                    await robot.CalibOutIOAsync(true);
                    //移动机器人至待机位置
                    await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
                    IsFinish = true;
                    break;
                }
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0, 0, 0);
                }
            }

            if (IsFinish)
            {
                Vector<double> p0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
                ToolCoord toolCoord = new ToolCoord();
                double u1 = calibrationInfo.CenterPoint.U;
                double u2 = RobotU.U;
                if (robot.Brand == Enums.RobotBrand.Schneider)
                {
                    u1 *= -1;
                    u2 *= -1;
                }
                toolCoord.ComputeTool(p0, p1, u1, u2);
                if (calibrationInfo.Tool == null)
                {
                    calibrationInfo.Tool = new RobotTool();
                }
                calibrationInfo.Tool.X = toolCoord.X;
                calibrationInfo.Tool.Y = toolCoord.Y;
                return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
            }
            else
            {
                return (false, null, 0, 0, 0, 0);
            }
        }

        /// <summary>
        /// J4 移动的向下相机校准工具坐标。
        /// 用于相机或工具随机器人第 4 轴（J4）运动时的标定，需考虑 J4 引入的偏转影响。
        /// </summary>
        /// <param name="calibrationInfo">标定所需配置信息。</param>
        /// <param name="robot">机器人接口。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>同 <see cref="CalibrateFixedDownCameraTool"/> 的返回约定。</returns>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateJ4MovingDownCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            //移动机器人至中心位置
            await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);

            //计算相机与机器人坐标系的旋转矩阵
            var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
            if (!robotCameraRotationMatrix.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
            pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
            RobotCameraRotationMatrix = robotCameraRotationMatrix.Item2;

            //自动移动机器人至图像中心点位置
            var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            calibrationInfo.CenterPoint.X = (float)autoMoveResult.X;
            calibrationInfo.CenterPoint.Y = (float)autoMoveResult.Y;

            RPoint RobotU = calibrationInfo.CenterPoint.Clone();
            RobotU.U += (float)calibrationInfo.Angle;

            //机器人U轴旋转
            await robot.CalibMotionAsync(RobotU, RobotU.Z);
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0, 0, 0);
            }

            //相机拍照并处理图像
            var photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            PointF curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));

            autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            RobotU.X = (float)autoMoveResult.X;
            RobotU.Y = (float)autoMoveResult.Y;

            Vector<double> p0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
            Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
            ToolCoord toolCoord = new ToolCoord();
            double u1 = calibrationInfo.CenterPoint.U;
            double u2 = RobotU.U;
            if (robot.Brand == Enums.RobotBrand.Schneider)
            {
                u1 *= -1;
                u2 *= -1;
            }
            toolCoord.ComputeTool(p0, p1, u1, u2);
            if (calibrationInfo.Tool == null)
            {
                calibrationInfo.Tool = new RobotTool();
            }

            calibrationInfo.Tool.X = toolCoord.X;
            calibrationInfo.Tool.Y = toolCoord.Y;

            return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
        }

        /// <summary>
        /// 固定向上相机校准工具坐标。
        /// 该方法用于相机固定、朝上安装的情况，标定过程与向下相机类似但需要考虑图像翻转/镜像等差异。
        /// </summary>
        /// <param name="calibrationInfo">标定配置信息。</param>
        /// <param name="robot">机器人接口。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>同 <see cref="CalibrateFixedDownCameraTool"/> 的返回约定。</returns>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateFixedUpCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            //移动机器人至中心位置
            await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);

            if (calibrationInfo.Pick.PickPlaceModel == Enums.PickPlaceModel.Inhal)
            {
                //打开吸气
                await robot.CalibOutIOAsync(true);
            }
            else
            {
                //张开夹爪
                await robot.CalibOutIOAsync(false);
            }

            var view = new ShowMessage(Lang.提示, Lang.请将校准块放置在吸嘴中心);
            var result = await DialogHost.Show(view, "CalibToolDialog", null, null, null);
            if (!((bool)result)) return (false, null, 0, 0, 0, 0);
            if (calibrationInfo.Pick.PickPlaceModel == Enums.PickPlaceModel.Clamp)
            {
                //夹紧夹爪
                await robot.CalibOutIOAsync(true);
            }

            //计算相机与机器人坐标系的旋转矩阵
            var robotCameraRotationMatrix= await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
            if (!robotCameraRotationMatrix.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            pixelScaleX= robotCameraRotationMatrix.PixelScaleX;
            pixelScaleY= robotCameraRotationMatrix.PixelScaleY;
            RobotCameraRotationMatrix= robotCameraRotationMatrix.Item2;

            //自动移动机器人至图像中心点位置
            var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            calibrationInfo.CenterPoint.X= (float)autoMoveResult.X;
            calibrationInfo.CenterPoint.Y= (float)autoMoveResult.Y;


            RPoint RobotU = calibrationInfo.CenterPoint.Clone();
            RobotU.U += (float)calibrationInfo.Angle;

            //机器人U轴旋转
            await robot.CalibMotionAsync(RobotU, RobotU.Z);
            if(cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0, 0, 0);
            }

            //相机拍照并处理图像
            var photoResult = await CaptureAndProcessCallback();
            if (!photoResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            PointF curPixel = new PointF((float)(photoResult.X), (float)(photoResult.Y));

            autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
            if (!autoMoveResult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            RobotU.X = (float)autoMoveResult.X;
            RobotU.Y = (float)autoMoveResult.Y;

            Vector<double> p0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
            Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
            ToolCoord toolCoord = new ToolCoord();
            double u1 = calibrationInfo.CenterPoint.U;
            double u2 = RobotU.U;
            if (robot.Brand == Enums.RobotBrand.Schneider)
            {
                u1 *= -1;
                u2 *= -1;
            }
            toolCoord.ComputeTool(p0, p1, u1, u2);
            if (calibrationInfo.Tool == null)
            {
                calibrationInfo.Tool = new RobotTool();
            }

            calibrationInfo.Tool.X = toolCoord.X;
            calibrationInfo.Tool.Y = toolCoord.Y;

            return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
        }

        /// <summary>
        /// 通过未标定的相机校准工具坐标。
        /// 当相机内参未知或未可靠标定时，通过已知机器人位姿与图像特征的对应关系反推工具坐标与像素尺度。
        /// </summary>
        /// <param name="calibrationInfo">标定配置信息（可能包含初始猜测或参考点）。</param>
        /// <param name="robot">机器人接口。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>同 <see cref="CalibrateFixedDownCameraTool"/> 的返回约定。</returns>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY)> CalibrateUncalibratedCameraTool(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            if (calibrationInfo == null) return (false, null, 0, 0, 0, 0);
            if (robot == null || !robot.IsConnected) return (false, null, 0, 0, 0, 0);

            double pixelScaleX = 0;
            double pixelScaleY = 0;
            Matrix<double> RobotCameraRotationMatrix = null;

            //发送校准的参数
            await robot.CalibParameAsync(calibrationInfo.Pick, calibrationInfo.Speed, calibrationInfo.Accel, calibrationInfo.Power, calibrationInfo.WaitSuction, calibrationInfo.WaitBlow);
            (bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY) calibresult;
            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
            {
                calibresult = await CalibrateFixedDownCameraTool(calibrationInfo,robot,cancellationToken);
            }
            else if (calibrationInfo.CameraMount == Enums.CameraMount.FixedUp)
            {
                calibresult = await CalibrateFixedUpCameraTool(calibrationInfo, robot, cancellationToken);
            }
            else if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
            {
                calibresult = await CalibrateJ4MovingDownCameraTool(calibrationInfo, robot, cancellationToken);
            }
            else
            {
                return (false, null, 0, 0, 0, 0);
            }
            if (!calibresult.IsSuccess)
            {
                return (false, null, 0, 0, 0, 0);
            }
            
            pixelScaleX= calibresult.PixelScaleX;
            pixelScaleY= calibresult.PixelScaleY;
            RobotCameraRotationMatrix= calibresult.RotationMatrix;

            return (true, RobotCameraRotationMatrix, calibresult.ToolX, calibresult.ToolY, pixelScaleX, pixelScaleY);
        }

        /// <summary>
        /// 自动识别机器人与相机图像坐标系的旋转矩阵。
        /// 使用若干已知的机器人位姿与对应图像坐标点，估计二者之间的旋转变换与像素尺度。
        /// </summary>
        /// <param name="calibrationInfo">包含用于识别的点集与图像处理配置。</param>
        /// <param name="robot">机器人接口，用于获取对应位姿或移动机器人到指定位置以采集数据。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>
        /// 元组字段：
        /// IsSuccess - 是否成功识别；
        /// Matrix<double> - 旋转（或仿射）矩阵；
        /// PixelScaleX - X 方向像素尺度（如 mm/px）；
        /// PixelScaleY - Y 方向像素尺度（如 mm/px）。
        /// </returns>
        public async Task<(bool IsSuccess, Matrix<double> RotationMatrix, double PixelScaleX, double PixelScaleY)> AutoIdentifyRobotCameraRotationMatrix(CalibrationInfo calibrationInfo, IRobot robot, CancellationToken cancellationToken)
        {
            //移动距离，单位mm
            float distance = ((float)calibrationInfo.Height) / 3;

            //-------------------------------------------P0-----------------------------------
            //移动机器人至中心点

            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
            {
                await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
                //关闭吸气
                await robot.CalibOutIOAsync(false);
            }
            else
            {
                await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
            }


            RPoint RobotP0 = await robot.GetRobotPosAsync();
            RPoint RobotP1 = RobotP0.Clone();
            RPoint RobotP2 = RobotP0.Clone();
            RobotP1.X += distance;
            RobotP2.Y += distance;

            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
            {
                //移动机器人至待机位置
                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
            }

            //相机拍照并处理图像
            var result = await CaptureAndProcessCallback();
            if (!result.IsSuccess)
            {
                return (false, null, 0, 0);
            }
            PointF P0 = new PointF((float)result.X, (float)result.Y);

            //-------------------------------------------P1-----------------------------------
            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
            {
                //移动机器人至P0
                await robot.CalibMotionAsync(RobotP0, 0);
                if(cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0);
                }
                //打开吸气
                await robot.CalibOutIOAsync(true);
                //移动机器人至P1
                await robot.CalibMotionAsync(RobotP1, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0);
                }
                //关闭吸气
                await robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
            }
            else
            {
                //移动机器人至P1
                await robot.CalibMotionAsync(RobotP1, RobotP1.Z);
            }

            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0);
            }

            //相机拍照并处理图像
            result = await CaptureAndProcessCallback();
            if (!result.IsSuccess)
            {
                return (false, null, 0, 0);
            }
            PointF P1 = new PointF((float)result.X, (float)result.Y);

            //-------------------------------------------P2-----------------------------------
            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
            {
                //移动机器人至P1
                await robot.CalibMotionAsync(RobotP1, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0);
                }
                //打开吸气
                await robot.CalibOutIOAsync(true);
                //移动机器人至P2
                await robot.CalibMotionAsync(RobotP2, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0);
                }
                //关闭吸气
                await robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
            }
            else
            {
                //移动机器人至P2
                await robot.CalibMotionAsync(RobotP2, RobotP2.Z);
            }
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0);
            }

            //相机拍照并处理图像
            result = await CaptureAndProcessCallback();
            if (!result.IsSuccess)
            {
                return (false, null, 0, 0);
            }

            PointF P2 = new PointF((float)result.X, (float)result.Y);

            //-------------------------------------------P0--------------------------------------
            if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
            {
                //移动机器人至P2
                await robot.CalibMotionAsync(RobotP2, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0);
                }
                //打开吸气
                await robot.CalibOutIOAsync(true);
                //移动机器人至P0
                await robot.CalibMotionAsync(RobotP0, 0);
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, null, 0, 0);
                }
                //关闭吸气
                await robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
            }
            else
            {
                //移动机器人至P0
                await robot.CalibMotionAsync(RobotP0, RobotP0.Z);
            }
            if (cancellationToken.IsCancellationRequested)
            {
                return (false, null, 0, 0);
            }

            //-----------------------------------------计算旋转矩阵------------------------------
            Matrix<double> M = CalculateImageRotationMatrix(P0, P1, P2);
            //Theta = Acos(M11)
            //相机与机器人坐标系角度：Theta * 180 / PI
            //-----------------------------------------计算像素和毫米比例-----------------------
            Vector<double> P11 = Vector<double>.Build.Dense(new double[] { distance, 0 });
            Vector<double> P21 = Vector<double>.Build.Dense(new double[] { P1.X, P1.Y }) - Vector<double>.Build.Dense(new double[] { P0.X, P0.Y });
            double PixelScaleX = Math.Abs((M * P11)[0] / P21[0]);       //mm/pixel

            double PixelScaleY = Math.Abs((M * P11)[1] / P21[1]);      //mm/pixel
            return (true, M, PixelScaleX, PixelScaleY);
        }

        /// <summary>
        /// 根据图像上的三个点计算图像坐标系的旋转矩阵。
        /// 通常通过已知三点在像素坐标系中的位置计算出局部旋转/仿射变换，用于后续坐标换算。
        /// </summary>
        /// <param name="P0">图像中的点 0（像素坐标）。</param>
        /// <param name="P1">图像中的点 1（像素坐标）。</param>
        /// <param name="P2">图像中的点 2（像素坐标）。</param>
        /// <returns>表示图像旋转或仿射变换的 3x3 或 2x2 矩阵（具体尺寸由实现决定）。</returns>
        public Matrix<double> CalculateImageRotationMatrix(PointF P0, PointF P1, PointF P2)
        {
            //1.将点转换为向量表示：
            Vector<double> Point0 = Vector<double>.Build.Dense(new double[] { (double)P0.X, (double)P0.Y });
            Vector<double> Point1 = Vector<double>.Build.Dense(new double[] { (double)P1.X, (double)P1.Y });
            Vector<double> Point2 = Vector<double>.Build.Dense(new double[] { (double)P2.X, (double)P2.Y });

            //2.计算坐标系的基向量：
            Vector<double> E1 = (Point1 - Point0) / (Point1 - Point0).L2Norm();
            Vector<double> E2 = (Point2 - Point0 - E1.DotProduct(Point2 - Point0) * E1) / (Point2 - Point0 - E1.DotProduct(Point2 - Point0) * E1).L2Norm();

            //3.计算旋转矩阵：我们可以使用基向量组成的矩阵作为旋转矩阵。具体地，我们可以将基向量 e1 和 e2 分别作为新坐标系下的 $x$ 和 $y$ 轴，然后组成一个矩阵，再对这个矩阵求逆即可得到旋转矩阵。代码实现：
            Matrix<double> R = Matrix<double>.Build.DenseOfColumnVectors(E1, E2);
            return R;
        }

        /// <summary>
        /// 自动移动机器人至图像中心点位置。
        /// 典型用例为将相机视野中心对应的工作点转换为机器人坐标并移动机器人到该点以便校准或抓取。
        /// </summary>
        /// <param name="robot">机器人接口，用于执行移动命令并返回当前位置。</param>
        /// <param name="matrix">图像坐标系到机器人坐标系的转换矩阵（旋转/仿射）。</param>
        /// <param name="PixelScaleX">像素到物理距离在 X 方向的缩放因子（如 mm/px）。</param>
        /// <param name="PixelScaleY">像素到物理距离在 Y 方向的缩放因子（如 mm/px）。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>
        /// 元组字段：
        /// IsSuccess - 是否成功移动到图像中心；
        /// X - 移动后机器人在基坐标系下的 X；
        /// Y - 移动后机器人在基坐标系下的 Y；
        /// U - 移动后机器人的朝向（工具角度）。
        /// </returns>
        public async Task<(bool IsSuccess, double X, double Y, double U)> AutoMoveRobotToImageCenter(IRobot robot, Matrix<double> matrix, double PixelScaleX, double PixelScaleY, CancellationToken cancellationToken)
        {
            bool IsFinish = false;

            //相机拍照并处理图像
            var result = await CaptureAndProcessCallback();
            if (!result.IsSuccess)
            {
                return (false, 0, 0, 0);
            }
            PointF curPixel = new PointF((float)result.X, (float)result.Y);
            //中心点像素坐标
            PointF P0 = new PointF((float)(result.ImageWidth / 2), (float)(result.ImageHeight / 2));

            var curpos = robot.GetRobotPos();
            for (int i = 0; i < 10; i++)
            {
                //计算需要移动的偏移量，目的是逼近中心点
                double offsetx = (P0.X - curPixel.X) * PixelScaleX;
                double offsety = (P0.Y - curPixel.Y) * PixelScaleY;
                var pos = matrix.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });
                curpos.X += (float)pos[0];
                curpos.Y += (float)pos[1];
                //机器人移动
                await robot.CalibMotionAsync(curpos, curpos.Z);
                await Task.Delay(200);

                result = await CaptureAndProcessCallback();
                if (!result.IsSuccess)
                {
                    return (false, 0, 0, 0);
                }
                curPixel = new PointF((float)result.X, (float)result.Y);

                offsetx = P0.X - curPixel.X;
                offsety = P0.Y - curPixel.Y;
                //await Console.Out.WriteLineAsync($"移动后的差异：{offsetx}，{offsety}");
                if (Math.Abs(offsetx) < 3 && Math.Abs(offsetx) < 3)
                {
                    IsFinish = true;
                    break;
                }
                if (cancellationToken.IsCancellationRequested)
                {
                    return (false, 0, 0, 0);
                }
            }
            if (IsFinish)
            {
                var finalPos = robot.GetRobotPos();
                return (true, finalPos.X, finalPos.Y, finalPos.U);
            }
            else
            {
                return (false, 0, 0, 0);
            }

        }

        /// <summary>
        /// 自动九点标定。
        /// 使用 9 个采样点进行仿射/透视变换求解，以提高标定精度并生成验证用的九点数据结构。
        /// </summary>
        /// <param name="calibrationInfo">九点标定的配置信息（点列、采集顺序、图像处理参数等）。</param>
        /// <param name="robot">机器人接口。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <returns>
        /// 元组字段：
        /// IsSuccess - 标定是否成功；
        /// AffineTransformationMaterial - 求解得到的仿射变换矩阵；
        /// NinePoint - 九点标定结果封装（像素点与对应的机器人位姿）；
        /// Result - 详细的标定结果与统计信息（误差、残差等）。
        /// </returns>
        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;
                if (calibrationInfo.CameraMount != Enums.CameraMount.MobileDown_XYPlatform)
                {
                    calibrationInfo.MarkPoint = calibrationInfo.CenterPoint.Clone();
                }
                
                calibrationInfo.RobotCameraRotationMatrix = robotCameraRotationMatrix.RotationMatrix.ToArray();
            }

            /// <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>
        /// 执行校准验证。
        /// 使用当前标定参数在一组验证点上计算误差并返回测试结果，用于评估标定质量。
        /// </summary>
        /// <param name="calibrationInfo">用于验证的点集与验收标准配置。</param>
        /// <param name="robot">机器人接口，用于在验证过程中获取或移动到参考位姿。</param>
        /// <param name="cancellationToken">用于取消操作的标记。</param>
        /// <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 if (calibrationInfo.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
                {
                    //像素直接转换成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);
        }
    }
}