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

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.Linq;
using System.Threading.Tasks;
using System.Windows;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Events;
using TeamAAS_VP.Core;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using static NPOI.POIFS.Crypt.CryptoFunctions;
using TeamAAS_VP;
using Cognex.VisionPro;
using System.Drawing;
using Cognex.VisionPro.ToolBlock;
using TeamAAS_VP.Resources.Languages;

namespace TeamAAS_VP.ViewModels.Calibration
{
    public class CalibrationAutoViewModel : BindableBase
    {
        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 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 _LoadedCommand;
        public DelegateCommand LoadedCommand =>
            _LoadedCommand ?? (_LoadedCommand = new DelegateCommand(ExecuteLoadedCommand));
        private DelegateCommand _NextCommand;
        public DelegateCommand NextCommand =>
            _NextCommand ?? (_NextCommand = new DelegateCommand(ExecuteNextCommand).ObservesCanExecute(() => IsRunning));

        private DelegateCommand _BackCommand;
        public DelegateCommand BackCommand =>
            _BackCommand ?? (_BackCommand = new DelegateCommand(ExecuteBackCommand).ObservesCanExecute(() => IsRunning));

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

        #endregion

        #region 事件

        #endregion

        public CalibrationAutoViewModel(IRegionManager regionManager, IEventAggregator ea, IRegionNavigationService regionNavigationService, IContainerProvider container, IDialogService dialogService)
        {
            _regionManager = regionManager;
            _regionNavigationService = regionNavigationService;
            _container = container;
            _dialogService = dialogService;
            _eventAggregator = ea;
            SelectCalibration = _container.Resolve<CalibrationHomeViewModel>().SelectCalibration;
        }

        #region 方法
        void ExecuteLoadedCommand()
        {
            try
            {
                if (_container.Resolve<Management>().CurrentUser.userPart == Enums.UserPart.Operator)
                {
                    IsAllowEdit = false;
                }
                else
                {
                    IsAllowEdit = true;
                }


                management = _container.Resolve<Management>();
                robotService = management.RobotService;
                SelectCalibration = _container.Resolve<CalibrationHomeViewModel>().SelectCalibration;
                if (SelectCalibration != null)
                {
                    Robot = robotService.GetRobot(SelectCalibration.RobotId);
                    if (SelectCalibration.FeederId != Guid.Empty)
                    {
                        Feeder = management.FeederService.GetFeeder(SelectCalibration.FeederId);
                    }
                }
                Camera = management.CameraService.GetCamera(SelectCalibration.CameraID);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("9点校准界面加载时出错!", ex);
                MessageBox.Show(ex.ToString());
            }
        }

        async void ExecuteStartCalibCommand()
        {
            try
            {
                if (MessageBox.Show(Lang.是否开始自动校准, $"{Lang.自动校准}", MessageBoxButton.OKCancel, MessageBoxImage.Question) != MessageBoxResult.OK) return;

                IsRunning = false;
                double IntervalX = SelectCalibration.Width / 2;
                double IntervalY = SelectCalibration.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 });

                //移动相机标定时，需要先记住特征点的绝对坐标（相对于机器人坐标系） P0
                //if (SelectCalibration.CameraMount == Enums.CameraMount.MobileJ4)
                //{
                //    P1[0] *=-1; P1[1] *= -1;
                //    P2[0] *= -1; P2[1] *= -1;
                //    P3[0] *= -1; P3[1] *= -1;
                //    P4[0] *= -1; P4[1] *= -1;
                //    P5[0] *= -1; P5[1] *= -1;
                //    P6[0] *= -1; P6[1] *= -1;
                //    P7[0] *= -1; P7[1] *= -1;
                //    P8[0] *= -1; P8[1] *= -1;
                //    P9[0] *= -1; P9[1] *= -1;
                //}

                var P0 = Vector<double>.Build.Dense(new double[] { SelectCalibration.CenterPoint.X, SelectCalibration.CenterPoint.Y });


                if (SelectCalibration.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
                {
                    var res = await AutoTechP0(SelectCalibration.CalibTechP0Mode);
                    if (!res)
                    {
                        MessageBox.Show(Lang.自动示教P0失败);
                        return;
                    }
                }

                /// <summary>
                /// 机器人与相机之间的旋转矩阵
                /// </summary>
                Matrix<double> RobotCameraRotationMatrix = Matrix<double>.Build.DenseOfArray(SelectCalibration.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 = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 2,
                    X = (float)P2[0],
                    Y = (float)P2[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 3,
                    X = (float)P3[0],
                    Y = (float)P3[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 4,
                    X = (float)P4[0],
                    Y = (float)P4[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 5,
                    X = (float)P5[0],
                    Y = (float)P5[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 6,
                    X = (float)P6[0],
                    Y = (float)P6[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 7,
                    X = (float)P7[0],
                    Y = (float)P7[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 8,
                    X = (float)P8[0],
                    Y = (float)P8[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                rPoints.Add(new RPoint()
                {
                    Number = 9,
                    X = (float)P9[0],
                    Y = (float)P9[1],
                    Z = SelectCalibration.CenterPoint.Z,
                    U = SelectCalibration.CenterPoint.U,
                    V = SelectCalibration.CenterPoint.V,
                    W = SelectCalibration.CenterPoint.W,
                    Hand = SelectCalibration.CenterPoint.Hand,
                    Local = SelectCalibration.CenterPoint.Local,
                    Tool = SelectCalibration.CenterPoint.Tool
                });
                List<PointF> PixelPoslist = new List<PointF>();
                if (SelectCalibration.CalibPoints == null)
                {
                    SelectCalibration.CalibPoints = new System.Collections.ObjectModel.ObservableCollection<RobotPixelPoint>();
                }
                else
                {
                    SelectCalibration.CalibPoints.Clear();
                }

                //移动相机标定时，需要先记住特征点的绝对坐标（相对于机器人坐标系） P0
                if (SelectCalibration.CameraMount == Enums.CameraMount.MobileJ4)
                {
                    await TechP0();
                }


                for (int i = 0; i < rPoints.Count; i++)
                {
                    //移动机器人至i
                    if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                    {
                        await Robot.CalibMotionAsync(rPoints[i], 0);
                        //关闭吸气
                        await Robot.CalibOutIOAsync(false);
                        //移动机器人至待机位置
                        await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);
                    }
                    else
                    {
                        await Robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
                    }
                    PixelPoslist.Add(Photo());

                    if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                    {
                        //移动机器人至i
                        await Robot.CalibMotionAsync(rPoints[i], 0);
                        //打开吸气
                        await Robot.CalibOutIOAsync(true);
                    }

                    SelectCalibration.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 (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                {
                    //移动机器人至待机位置
                    await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);
                }

                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 (SelectCalibration.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, SelectCalibration.MarkPoint.X - ox);
                        matRobot.Set<double>(i, 1, SelectCalibration.MarkPoint.Y - oy);
                    }
                    else if (SelectCalibration.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, SelectCalibration.MarkPoint.X - ox);
                        matRobot.Set<double>(i, 1, SelectCalibration.MarkPoint.Y - oy);
                    }
                    else
                    {
                        //将tool 0下的坐标转换成Tool n下的坐标
                        if (SelectCalibration.Tool != null)
                        {
                            ToolCoord toolCoord = new ToolCoord();
                            toolCoord.SetTool(SelectCalibration.Tool.X, SelectCalibration.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);
                SelectCalibration.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 (SelectCalibration.CameraMount == Enums.CameraMount.MobileJ4)
                    {
                        //计算每个点相对于第一个点移动的偏移量，Mark点减去这个偏移量及是图像中对应的实际坐标
                        double ox = rPoints[i].X - rPoints[0].X;
                        double oy = rPoints[i].Y - rPoints[0].Y;
                        differenceX.Add((SelectCalibration.MarkPoint.X - ox) - (matrix * pixel)[0]);
                        differenceY.Add((SelectCalibration.MarkPoint.Y - oy) - (matrix * pixel)[1]);
                    }
                    else if (SelectCalibration.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
                    {
                        double ox = rPoints[i].X - rPoints[0].X;
                        double oy = rPoints[i].Y - rPoints[0].Y;
                        differenceX.Add((SelectCalibration.MarkPoint.X - ox) - (matrix * pixel)[0]);
                        differenceY.Add((SelectCalibration.MarkPoint.Y - oy) - (matrix * pixel)[1]);
                    }
                    else
                    {
                        //将tool 0下的坐标转换成Tool n下的坐标
                        if (SelectCalibration.Tool != null)
                        {
                            ToolCoord toolCoord = new ToolCoord();
                            toolCoord.SetTool(SelectCalibration.Tool.X, SelectCalibration.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();
                SelectCalibration.CalibrationResult = calibrationResult;

                DialogParameters parameters = new DialogParameters();
                parameters.Add("Result", calibrationResult);
                _dialogService.Show("ShowCalibrationResult", parameters, rst =>
                {
                    //对话框关闭之后的回调函数，可以在这解析结果。
                    ButtonResult result1 = rst.Result;

                    if (result1 == ButtonResult.OK)
                    {
                        _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationVerification");
                    }

                });

                //MessageBox.Show($"校准完成:RmsX:{rmsX}-RmsY:{rmsY}");
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("9点校准时出错!", ex);
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning = true;
            }
        }

        /// <summary>
        /// 记录P0点（移动相机校准时有效）
        /// </summary>
        async Task TechP0()
        {
            //移动机器人至中心位置
            await Robot.CalibMotionAsync(SelectCalibration.CenterPoint, 0);

            //将当前机器人Tool 0下的坐标转换成工具坐标下的坐标，并记录下来
            ToolCoord toolCoord = new ToolCoord();
            toolCoord.SetTool(SelectCalibration.Tool.X, SelectCalibration.Tool.Y);
            Vector<double> p = Vector<double>.Build.Dense(new double[] { SelectCalibration.CenterPoint.X, SelectCalibration.CenterPoint.Y }); ;
            var pn = toolCoord.GetToolnCoord(p, SelectCalibration.CenterPoint.U);
            if (SelectCalibration.MarkPoint == null)
            {
                SelectCalibration.MarkPoint = new RPoint();
            }
            SelectCalibration.MarkPoint = SelectCalibration.CenterPoint.Clone();
            SelectCalibration.MarkPoint.X = (float)pn[0];
            SelectCalibration.MarkPoint.Y = (float)pn[1];
        }

        /// <summary>
        /// 下一步
        /// </summary>
        void ExecuteNextCommand()
        {
            _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationVerification");
        }

        /// <summary>
        /// 上一步
        /// </summary>
        void ExecuteBackCommand()
        {
            if (SelectCalibration.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
            {
                _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationTeachCenterPoint");
            }
            else
            {
                _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationSelectTool");
            }
        }

        /// <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>
        /// 自动示教P0点
        /// </summary>
        /// <returns></returns>
        private async Task<bool> AutoTechP0(Enums.CalibTechP0Mode calibTechP0)
        {
            // 移动机器人至中心位置
            await Robot.CalibMotionAsync(SelectCalibration.CenterPoint, 0);
            //计算相机与机器人坐标系的旋转矩阵
            var rst = await MoveRobotThreePointComputCroodTrans(SelectCalibration.CenterPoint, ((float)SelectCalibration.Height) / 3);
            SelectCalibration.RobotCameraRotationMatrix = rst.Item1.ToArray();
            SelectCalibration.PixelScaleX = rst.Item2;
            SelectCalibration.PixelScaleY = rst.Item3;
            if (calibTechP0== Enums.CalibTechP0Mode.TechP0)
            {
                return true;
            }

            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();
                SelectCalibration.MarkPoint= SelectCalibration.CenterPoint.Clone();
                return true;
            }
            else
            {
                return false;
            }
        }
        /// <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;
        }
        #endregion

        private bool _IsAllowEdit = false;

        public bool IsAllowEdit
        {
            get { return _IsAllowEdit; }
            set { SetProperty(ref _IsAllowEdit, value); }
        }

        private void LoginChange(Models.User user)
        {
            if (user.userPart == Enums.UserPart.Operator)
            {
                IsAllowEdit = false;
            }
            else
            {
                IsAllowEdit = true;
            }
        }
    }
}