Display_D_HP/TeamAAS-VM/ViewModels/Calibration/CalibrationVerificationViewModel.cs

using Cognex.VisionPro;
using Cognex.VisionPro.ToolBlock;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Complex;
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.Threading.Tasks;
using System.Windows;
using Team.FFFeederService.Interfaces;
using TeamAAS_VP.Core;
using TeamAAS_VP.Events;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP;
using TeamAAS_VP.Resources.Languages;
using TeamAAS_VP.Models.Calibration;
using TeamAAS_VP.Services;

namespace TeamAAS_VP.ViewModels.Calibration
{
    public class CalibrationVerificationViewModel : BindableBase, IConfirmNavigationRequest
    {
        IRegionManager _regionManager;
        IRegionNavigationService _regionNavigationService;
        IContainerProvider _container;
        IDialogService _dialogService;
        IEventAggregator _eventAggregator;
        IConfigService _configService;
        IRobotService _robotService;
        ICameraService _cameraService;
        IFeederService _feederService;
        ISystemDatabaseService _systemDatabaseService;

        #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 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 _StartTestCommand;
        public DelegateCommand StartTestCommand =>
            _StartTestCommand ?? (_StartTestCommand = new DelegateCommand(ExecuteStartTestCommand).ObservesCanExecute(() => IsRunning));

        
        #endregion

        #region 事件

        #endregion

        public CalibrationVerificationViewModel(IRegionManager regionManager, IEventAggregator ea, IRegionNavigationService regionNavigationService, IContainerProvider container, IDialogService dialogService,
            IConfigService configService, IRobotService robotService, ICameraService cameraService, IFeederService feederService, ISystemDatabaseService systemDatabaseService)
        {
            _regionManager = regionManager;
            _regionNavigationService = regionNavigationService;
            _container = container;
            _dialogService = dialogService;
            _eventAggregator = ea;
            _configService = configService;
            _robotService = robotService;
            _cameraService = cameraService;
            _feederService = feederService;
            _systemDatabaseService = systemDatabaseService;
        }

        #region 方法
        void ExecuteLoadedCommand()
        {
            if (_systemDatabaseService.GetCurrentUser().userPart == Enums.UserPart.Operator)
            {
                IsAllowEdit = false;
            }
            else
            {
                IsAllowEdit = true;
            }
        }

        /// <summary>
        /// 测试校准
        /// </summary>
        async void ExecuteStartTestCommand()
        {
            try
            {
                if (MessageBox.Show(Lang.是否开始测试, Lang.自动校准, MessageBoxButton.OKCancel, MessageBoxImage.Question) != MessageBoxResult.OK) return;
                IsRunning = false;
                double IntervalX = SelectCalibration.Width*0.90 / 2;
                double IntervalY = SelectCalibration.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 });

                //移动相机标定时，需要先记住特征点的绝对坐标（相对于机器人坐标系） 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;
                //}

                var P0 = Vector<double>.Build.Dense(new double[] { SelectCalibration.CenterPoint.X, SelectCalibration.CenterPoint.Y });
                /// <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;
                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
                });

                List<Vector<double>> TestPos = new List<Vector<double>>();
                for (int i = 0; i < rPoints.Count; i++)
                {
                    if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                    {
                        //移动机器人至i
                        await Robot.CalibMotionAsync(rPoints[i], 0);
                        //关闭吸气
                        await Robot.CalibOutIOAsync(false);
                        //移动机器人至待机位置
                        await Robot.CalibMotionAsync(SelectCalibration.HomePoint, 0);
                    }
                    else
                    {
                        //移动机器人至i
                        await Robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
                    }
                    var pixelpos = Photo();
                    Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(SelectCalibration.AffineTransformationMaterial);
                    var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelpos.X, pixelpos.Y ,1});
                    TestPos.Add(rpos);
                    if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                    {
                        //移动机器人至i
                        await Robot.CalibMotionAsync(rPoints[i], 0);
                        //打开吸气
                        await Robot.CalibOutIOAsync(true);
                    }
                }

                if (SelectCalibration.CameraMount == Enums.CameraMount.FixedDown)
                {
                    //移动机器人至待机位置
                    await Robot.CalibMotionAsync(SelectCalibration.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 (SelectCalibration.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 - SelectCalibration.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 - SelectCalibration.CalibPoints[0].Robot.X, Robot_Y - SelectCalibration.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] - SelectCalibration.MarkPoint.X);
                        differenceY.Add(cc[1] - SelectCalibration.MarkPoint.Y);
                    }
                    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] - 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;

                SelectCalibration.CalibrationTestResult = calibrationTestResult;
                MessageBox.Show(Lang.验证完成);
                NavigationParameters param = new NavigationParameters();
                param.Add("SelectCalibration", SelectCalibration);
                _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationResult", param);
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("相机校准-校准验证界面示加载时出错!", ex);
                _eventAggregator.GetEvent<SnackbarMessageNotification>().Publish(new MessageParameter() { Msg = ex.Message, Duration = 1 });
                MessageBox.Show(ex.Message);
            }
            finally
            {
                IsRunning = true;
            }
        }

        /// <summary>
        /// 下一步
        /// </summary>
        void ExecuteNextCommand()
        {
            NavigationParameters param = new NavigationParameters();
            param.Add("SelectCalibration", SelectCalibration);
            _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationResult", param);
        }

        /// <summary>
        /// 上一步
        /// </summary>
        void ExecuteBackCommand()
        {
            NavigationParameters param = new NavigationParameters();
            param.Add("SelectCalibration", SelectCalibration);
            _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationAuto", param);
        }

        /// <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.拍照失败);
            }
        }
        #endregion

        #region 继承
        /// <summary>
        /// 确认导航请求时调用。此方法允许您在导航之前执行一些操作。
        /// </summary>
        /// <param name="navigationContext"></param>
        /// <param name="continuationCallback"></param>
        public void ConfirmNavigationRequest(NavigationContext navigationContext, Action<bool> continuationCallback)
        {
            continuationCallback(true);
        }

        /// <summary>
        /// 接收导航请求时调用。传入导航参数，包含有关导航目标的信息。
        /// </summary>
        /// <param name="navigationContext"></param>
        /// <exception cref="NotImplementedException"></exception>
        public async void OnNavigatedTo(NavigationContext navigationContext)
        {
            SelectCalibration = navigationContext.Parameters.GetValue<CalibrationInfo>("SelectCalibration");
            if (SelectCalibration != null)
            {
                Robot = _robotService.GetRobot(SelectCalibration.RobotId);
                if (SelectCalibration.FeederId != Guid.Empty)
                {
                    Feeder = _feederService.GetFeeder(SelectCalibration.FeederId);
                }

            }
            Camera = _cameraService.GetCamera(SelectCalibration.CameraID);
        }

        /// <summary>
        /// 是否允许导航到此视图模型。
        /// </summary>
        /// <param name="navigationContext"></param>
        /// <returns></returns>
        public bool IsNavigationTarget(NavigationContext navigationContext)
        {
            return true;
        }

        /// <summary>
        /// 导航离开此视图模型时调用。您可以在此处执行清理操作或保存状态。
        /// </summary>
        /// <param name="navigationContext"></param>
        public void OnNavigatedFrom(NavigationContext navigationContext)
        {

        }
        #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;
            }
        }
    }
}