TeamAAS-VP-HeShuo/TeamAAS-VM/ViewModels/Calibration/CalibToolViewModel.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MaterialDesignThemes.Wpf;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using NPOI.SS.Formula.Functions;
using OpenCvSharp;
using Prism.Commands;
using Prism.Events;
using Prism.Ioc;
using Prism.Mvvm;
using Prism.Regions;
using Prism.Services.Dialogs;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Reflection.Metadata;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media.Media3D;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Controls;
using TeamAAS_VP.Core;
using TeamAAS_VP.Events;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP;
using static OpenCvSharp.LineIterator;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.ViewModels.Calibration
{
    public class CalibToolViewModel : BindableBase, IDialogAware
    {
        IRegionManager _regionManager;
        IRegionNavigationService _regionNavigationService;
        IContainerProvider _container;
        IDialogService _dialogService;
        IEventAggregator _eventAggregator;

        #region 属性

        private ICogImage _Image;

        public ICogImage Image
        {
            get { return _Image; }
            set { SetProperty(ref _Image, value); }
        }

        private Cognex.VisionPro.CogGraphicCollection _Graphic;

        public Cognex.VisionPro.CogGraphicCollection Graphic
        {
            get { return _Graphic; }
            set { SetProperty(ref _Graphic, value); }
        }

        private CalibrationInfo _SelectCalibration;
        
        public CalibrationInfo SelectCalibration
        {
            get { return _SelectCalibration; }
            set { SetProperty(ref _SelectCalibration, value); }
        }

        public string Title=> "校准工具坐标";

        private double _Angle = 180;
        /// <summary>
        /// 机器人移动的范围U
        /// </summary>
        public double Angle
        {
            get { return _Angle; }
            set { SetProperty(ref _Angle, value); }
        }

        public RobotTool Tool { get; set; }
        /// <summary>
        /// 机器人与相机之间的旋转矩阵
        /// </summary>
        public Matrix<double> RobotCameraRotationMatrix {  get; set; }  
        public Management management { get; set; }
        public RobotService robotService { get; set; }
        public IRobot Robot { get; set; }
        public IVoiceCoilMotorFeeder Feeder { get; set; }

        private bool _IsRunning=true;

        public bool IsRunning
        {
            get { return _IsRunning; }
            set { SetProperty(ref _IsRunning,value); }
        }

        private ICamera _Camera;

        public ICamera Camera
        {
            get { return _Camera; }
            set { SetProperty(ref _Camera, value); }
        }
        #endregion

        #region 命令
        private DelegateCommand _ConfirmCommand;
        public DelegateCommand ConfirmCommand =>
            _ConfirmCommand ?? (_ConfirmCommand = new DelegateCommand(ExecuteConfirmCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _CancelCommand;
        public DelegateCommand CancelCommand =>
            _CancelCommand ?? (_CancelCommand = new DelegateCommand(ExecuteCancelCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _StartCalibCommand;
        public DelegateCommand StartCalibCommand =>
            _StartCalibCommand ?? (_StartCalibCommand = new DelegateCommand(ExecuteStartCalibCommand).ObservesCanExecute(()=> IsRunning));

        
        #endregion

        #region 事件
        public event Action<IDialogResult> RequestClose;
        #endregion

        public CalibToolViewModel(IRegionManager regionManager, IEventAggregator ea, IRegionNavigationService regionNavigationService, IContainerProvider container, IDialogService dialogService)
        {
            _regionManager = regionManager;
            _regionNavigationService = regionNavigationService;
            _container = container;
            _dialogService = dialogService;
            _eventAggregator = ea;
        }

        #region 方法

        /// <summary>
        /// 确定
        /// </summary>
        void ExecuteConfirmCommand()
        {
            if (Tool==null)
            {
                return;
            }

            IDialogParameters parameters = new DialogParameters();
            parameters.Add("Tool", Tool);
            parameters.Add("RobotCameraRotationMatrix", RobotCameraRotationMatrix);
            parameters.Add("Angle", Angle);
            RequestClose?.Invoke(new DialogResult(ButtonResult.OK, parameters));
        }
        
        /// <summary>
        /// 取消
        /// </summary>
        void ExecuteCancelCommand()
        {
            IDialogParameters parameters = new DialogParameters();
            parameters.Add("Tool", Tool);
            RequestClose?.Invoke(new DialogResult(ButtonResult.Cancel, parameters));
        }

        /// <summary>
        /// 开始校准
        /// </summary>
        async void ExecuteStartCalibCommand()
        {
            if (SelectCalibration == null) return;
            if (Robot == null || !Robot.IsConnected) return;
            Robot.Timeout = 600000;
            try
            {
                if (Angle == 0)
                    Angle = 10;
                var view = new ShowMessage(Lang.Tool校准, Lang.是否开始自动校准工具坐标);
                //show the dialog
                var result = await DialogHost.Show(view, "CalibToolDialog", null, null, null);
                if (!((bool)result)) return;
                IsRunning = false;
                //发送校准的参数
                await Robot.CalibParameAsync(SelectCalibration.Pick, SelectCalibration.Speed, SelectCalibration.Accel, SelectCalibration.Power, SelectCalibration.WaitSuction, SelectCalibration.WaitBlow);

                Tuple<bool, Matrix<double>, double, double> calibresult=new Tuple<bool, Matrix<double>, double, double>(false,null,0,0);
                if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                {
                    calibresult = await CalibToolForFixedDown();
                }
                else if (SelectCalibration.CameraMount == Enums.CameraMount.FixedUp)
                {
                    calibresult = await CalibToolForFixedUp();
                }
                else if (SelectCalibration.CameraMount == Enums.CameraMount.MobileJ4)
                {
                    calibresult = await CalibToolForMobileJ4();
                }
                else
                {
                    view = new ShowMessage(Lang.Tool校准, Lang.暂不支持此安装方式的相机, false);
                    result = await DialogHost.Show(view, "CalibToolDialog", null, null, null);
                    return;
                }
                
                if (calibresult.Item1)
                {
                    if (Tool==null)
                    {
                        Tool = new RobotTool();
                    }
                    Tool.X = calibresult.Item3;
                    Tool.Y = calibresult.Item4;
                    RobotCameraRotationMatrix = calibresult.Item2;

                    //MessageBox.Show("校准完成");
                    view = new ShowMessage(Lang.Tool校准, Lang.校准完成, false);
                    await DialogHost.Show(view, "CalibToolDialog", null, null, null);

                    IDialogParameters parameters = new DialogParameters();
                    parameters.Add("Tool", Tool);
                    parameters.Add("RobotCameraRotationMatrix", RobotCameraRotationMatrix);
                    RequestClose?.Invoke(new DialogResult(ButtonResult.OK, parameters));
                }
                else
                {
                    view = new ShowMessage(Lang.Tool校准, Lang.校准失败, false);
                    //show the dialog
                    await DialogHost.Show(view, "CalibToolDialog", null, null, null);
                    
                }
                IsRunning = true;
            }
            catch (Exception ex)
            {
                IsRunning = true;
                LogHelper.WriteLogError("相机校准-校准验证界面示加载时出错!", ex);
                //MessageBox.Show(ex.Message);
                var view = new ShowMessage(Lang.Tool校准, $"{ex.Message}", false);
                //show the dialog
                var result = await DialogHost.Show(view, "CalibToolDialog", null, null, null);
            }
        }

        public bool CanCloseDialog()
        {
            return true;
        }

        public void OnDialogClosed()
        {
            
        }

        public void OnDialogOpened(IDialogParameters parameters)
        {
            SelectCalibration = _container.Resolve<CalibrationHomeViewModel>().SelectCalibration;
            management = _container.Resolve<Management>();
            robotService = management.RobotService;
            if (SelectCalibration != null)
            {
                Camera = management.CameraService.GetCamera(SelectCalibration.CameraID);
                Robot = robotService.GetRobot(SelectCalibration.RobotId);
                if (SelectCalibration.FeederId!=Guid.Empty)
                {
                    Feeder = management.FeederService.GetFeeder(SelectCalibration.FeederId);
                }
                Angle = SelectCalibration.Angle;
            }
        }

        /// <summary>
        /// 移动机器人3个点计算旋转矩阵
        /// </summary>
        /// <returns>(旋转矩阵,PixelScaleX,PixelScaleY)</returns>
        private async Task<Tuple<Matrix<double>,double,double>> MoveRobotThreePointComputCroodTrans(RPoint rPoint, float distance)
        {
            //-------------------------------------------P0-----------------------------------
            //移动机器人至中心点

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

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

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

            var P0=Photo();
            //-------------------------------------------P1-----------------------------------
            if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
            {
                //移动机器人至P0
                await Robot.CalibMotionAsync(RobotP0, 0);
                //打开吸气
                await Robot.CalibOutIOAsync(true);
                //移动机器人至P1
                await Robot.CalibMotionAsync(RobotP1, 0);
                //关闭吸气
                await Robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);
            }
            else
            {
                //移动机器人至P1
                await Robot.CalibMotionAsync(RobotP1, RobotP1.Z);
            }
            var P1 = Photo();
            
            //-------------------------------------------P2-----------------------------------
            if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
            {
                //移动机器人至P1
                await Robot.CalibMotionAsync(RobotP1, 0);
                //打开吸气
                await Robot.CalibOutIOAsync(true);
                //移动机器人至P2
                await Robot.CalibMotionAsync(RobotP2, 0);
                //关闭吸气
                await Robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);
            }
            else
            {
                //移动机器人至P2
                await Robot.CalibMotionAsync(RobotP2, RobotP2.Z);
            }
            var P2 = Photo();

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

            //-----------------------------------------计算旋转矩阵------------------------------
            Matrix<double> M= ComputRotationMatrix(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 new Tuple<Matrix<double>, double, double>(M, PixelScaleX, PixelScaleY);
        }

        /// <summary>
        /// 计算旋转矩阵
        /// </summary>
        /// <param name="P0"></param>
        /// <param name="P1"></param>
        /// <param name="P2"></param>
        /// <returns></returns>
        private Matrix<double> ComputRotationMatrix(PointF P0, PointF P1, PointF P2)
        {
            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$ 轴，然后组成一个矩阵，再对这个矩阵求逆即可得到旋转矩阵。代码实现：
            // Compute rotation matrix
            Matrix<double>  R = Matrix<double>.Build.DenseOfColumnVectors(E1, E2);
            return R;
        }

        /// <summary>
        /// 相机拍照
        /// </summary>
        /// <returns></returns>
        /// <exception cref="Exception"></exception>
        private PointF Photo()
        {
            if (Feeder != null)
            {
                Feeder.OpenLight();
                Feeder.SetLightTimeOut((ushort)SelectCalibration.Brightness);
            }
            Task.Delay(200).Wait();
            //采集图像
            if (Camera != null)
            {
                SelectCalibration.ToolBlock.Inputs["InputImage"].Value = Camera.Grab();
                Image = (ICogImage)SelectCalibration.ToolBlock.Inputs["InputImage"].Value;
            }
            Graphic = null;
            SelectCalibration.ToolBlock.Run();
            CogToolBlockTerminalCollection outputCollection = SelectCalibration.ToolBlock.Outputs;
            Graphic = outputCollection["Graphic"].Value as CogGraphicCollection;
            bool Found = (bool)(outputCollection["Found"].Value);
            if (Found)
            {
                var strpoints = outputCollection["Point"].Value.ToString();
                string[] items = strpoints.Split(',');
                return new PointF(float.Parse(items[0]), float.Parse(items[1]));
            }
            else
            {
                throw new Exception(Lang.拍照失败);
            }
        }

        /// <summary>
        /// 固定向下相机校准工具坐标
        /// </summary>
        /// <returns>（结果，旋转矩阵，Tool.X，Tool.Y）</returns>
        private async Task<Tuple<bool,Matrix<double>, double, double>> CalibToolForFixedDown()
        {
            //移动机器人至待机位置
            await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);

            
            if (SelectCalibration.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)) { IsRunning = true; return new Tuple<bool, Matrix<double>, double, double>(false,null,0,0); }
            if (SelectCalibration.Pick.PickPlaceModel == Enums.PickPlaceModel.Clamp)
            {
                //夹紧夹爪
                await Robot.CalibOutIOAsync(true);
            }

            //计算相机与机器人坐标系的旋转矩阵
            var rst = await MoveRobotThreePointComputCroodTrans(SelectCalibration.CenterPoint, ((float)SelectCalibration.Height) / 3);
            SelectCalibration.PixelScaleX = rst.Item2;
            SelectCalibration.PixelScaleY = rst.Item3;
            var P0 = Photo();       //拍照获取中点像素坐标

            RPoint RobotU = SelectCalibration.CenterPoint.Clone();
            RobotU.U += (float)Angle;
            //移动机器人中心点
            await Robot.CalibMotionAsync(SelectCalibration.CenterPoint, 0);
            //打开吸气
            await Robot.CalibOutIOAsync(true);

            //机器人U轴旋转
            await Robot.CalibMotionAsync(RobotU, 0);

            //关闭吸气
            await Robot.CalibOutIOAsync(false);
            //移动机器人至待机位置
            await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);

            var curPixel = Photo();

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

                RobotU.X += (float)pos[0];
                RobotU.Y += (float)pos[1];
                //机器人移动
                await Robot.CalibMotionAsync(RobotU, 0);

                //关闭吸气
                await Robot.CalibOutIOAsync(false);
                //移动机器人至待机位置
                await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);

                curPixel = Photo();
                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);
                    //打开吸气
                    await Robot.CalibOutIOAsync(true);
                    //移动机器人至待机位置
                    await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);
                    IsFinish = true;
                    break;
                }
            }

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

        /// <summary>
        /// 固定向上相机校准工具坐标
        /// </summary>
        /// <returns>（结果，旋转矩阵，Tool.X，Tool.Y）</returns>
        private async Task<Tuple<bool, Matrix<double>, double, double>> CalibToolForFixedUp()
        {
            //移动机器人至中心位置
            await Robot.CalibMotionAsync(SelectCalibration.CenterPoint, 0);

            if (SelectCalibration.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)) { IsRunning = true; return new Tuple<bool, Matrix<double>, double, double>(false,null,0,0); }
            if (SelectCalibration.Pick.PickPlaceModel == Enums.PickPlaceModel.Clamp)
            {
                //夹紧夹爪
                await Robot.CalibOutIOAsync(true);
            }

            //计算相机与机器人坐标系的旋转矩阵
            var rst = await MoveRobotThreePointComputCroodTrans(SelectCalibration.CenterPoint, ((float)SelectCalibration.Height) / 3);
            SelectCalibration.PixelScaleX = rst.Item2;
            SelectCalibration.PixelScaleY = rst.Item3;
            var imagecenter = Camera.Grab();
            PointF P0 = new PointF((float)(imagecenter.Width/2), (float)(imagecenter.Height / 2));       //中心点像素坐标

            //先找相机中心点，校准中心点坐标
            bool IsFinish = false;
            var curPixel = Photo();
            var curpos = Robot.GetRobotPos();
            for (int i = 0; i < 10; i++)
            {
                //计算需要移动的偏移量，目的是逼近中心点
                double offsetx = (P0.X - curPixel.X) * rst.Item2;
                double offsety = (P0.Y - curPixel.Y) * rst.Item3;
                var pos = rst.Item1.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);

                curPixel = Photo();
                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)
                {
                    IsFinish = true;
                    break;
                }
            }
            if (IsFinish==true)
            {
                SelectCalibration.CenterPoint= Robot.GetRobotPos();
            }
            else
            {
                return new Tuple<bool, Matrix<double>, double, double>(false, null, 0, 0);
            }

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

            //机器人U轴旋转
            await Robot.CalibMotionAsync(RobotU, RobotU.Z);
            curPixel = Photo();


            IsFinish = false;
            for (int i = 0; i < 10; i++)
            {
                //计算需要移动的偏移量，目的是逼近中心点
                double offsetx = (P0.X - curPixel.X) * rst.Item2;
                double offsety = (P0.Y - curPixel.Y) * rst.Item3;
                var pos = rst.Item1.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });

                RobotU.X += (float)pos[0];
                RobotU.Y += (float)pos[1];
                //机器人移动
                await Robot.CalibMotionAsync(RobotU, 0);

                curPixel = Photo();
                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)
                {
                    IsFinish = true;
                    break;
                }
            }

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

                /*double toolx= toolCoord.X;
                if (Robot.Brand == Enums.RobotBrand.Schneider)
                {
                    toolx *= -1;
                }*/
                Tool.X = toolCoord.X;
                Tool.Y = toolCoord.Y;
                RobotCameraRotationMatrix = rst.Item1;
                return new Tuple<bool, Matrix<double>, double, double>(true, RobotCameraRotationMatrix, Tool.X, Tool.Y);
            }
            else
            {
                return new Tuple<bool, Matrix<double>, double, double>(false, null, 0, 0);
            }
        }

        /// <summary>
        /// J4移动机器人校准工具坐标
        /// </summary>
        /// <returns>（结果，旋转矩阵，Tool.X，Tool.Y）</returns>
        private async Task<Tuple<bool, Matrix<double>, double, double>> CalibToolForMobileJ4()
        {
            //移动机器人至中心位置
            await Robot.CalibMotionAsync(SelectCalibration.CenterPoint, 0);

            //计算相机与机器人坐标系的旋转矩阵
            var rst = await MoveRobotThreePointComputCroodTrans(SelectCalibration.CenterPoint, ((float)SelectCalibration.Height) / 3);
            SelectCalibration.PixelScaleX = rst.Item2;
            SelectCalibration.PixelScaleY = rst.Item3;

            var imagecenter = Camera.Grab();
            PointF P0 = new PointF((float)(imagecenter.Width / 2), (float)(imagecenter.Height / 2));       //中心点像素坐标

            //先找相机中心点，校准中心点坐标
            bool IsFinish = false;
            var curPixel = Photo();
            var curpos = Robot.GetRobotPos();
            for (int i = 0; i < 10; i++)
            {
                //计算需要移动的偏移量，目的是逼近中心点
                double offsetx = (P0.X - curPixel.X) * rst.Item2;
                double offsety = (P0.Y - curPixel.Y) * rst.Item3;
                var pos = rst.Item1.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);

                curPixel = Photo();
                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)
                {
                    IsFinish = true;
                    break;
                }
            }
            if (IsFinish == true)
            {
                SelectCalibration.CenterPoint = Robot.GetRobotPos();
            }
            else
            {
                return new Tuple<bool, Matrix<double>, double, double>(false, null, 0, 0);
            }

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

            //机器人U轴旋转
            await Robot.CalibMotionAsync(RobotU, RobotU.Z);
            //计算相机与机器人坐标系的旋转矩阵
            var rst1 = await MoveRobotThreePointComputCroodTrans(RobotU, 1);

            curPixel = Photo();

            IsFinish = false;
            for (int i = 0; i < 10; i++)
            {
                //计算需要移动的偏移量，目的是逼近中心点
                double offsetx = (P0.X - curPixel.X) * rst1.Item2;
                double offsety = (P0.Y - curPixel.Y) * rst1.Item3;
                var pos = rst1.Item1.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });

                RobotU.X += (float)pos[0];
                RobotU.Y += (float)pos[1];
                //机器人移动
                await Robot.CalibMotionAsync(RobotU, RobotU.Z);

                curPixel = Photo();
                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)
                {
                    IsFinish = true;
                    break;
                }
            }

            if (IsFinish)
            {
                Vector<double> p0 = Vector<double>.Build.Dense(new double[] { SelectCalibration.CenterPoint.X, SelectCalibration.CenterPoint.Y });
                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { RobotU.X, RobotU.Y });
                ToolCoord toolCoord = new ToolCoord();
                double u1 = SelectCalibration.CenterPoint.U;
                double u2 = RobotU.U;
                if (Robot.Brand == Enums.RobotBrand.Schneider)
                {
                    u1 *= -1;
                    u2 *= -1;
                }
                toolCoord.ComputeTool(p0, p1, u1, u2);
                if (Tool == null)
                {
                    Tool = new RobotTool();
                }
                Tool.X = toolCoord.X;
                Tool.Y = toolCoord.Y;
                RobotCameraRotationMatrix = rst.Item1;
                return new Tuple<bool, Matrix<double>, double, double>(true, RobotCameraRotationMatrix, Tool.X, Tool.Y);
            }
            else
            {
                return new Tuple<bool, Matrix<double>, double, double>(false, null, 0, 0);
            }
        }
        #endregion
    }
}