TeamAAS-VP-HeShuo/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;

namespace TeamAAS_VP.ViewModels.Calibration
{
    public class CalibrationVerificationViewModel : 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 _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)
        {
            _regionManager = regionManager;
            _regionNavigationService = regionNavigationService;
            _container = container;
            _dialogService = dialogService;
            _eventAggregator = ea;
            SelectCalibration = _container.Resolve<CalibrationHomeViewModel>().SelectCalibration;
        }

        #region 方法
        void ExecuteLoadedCommand()
        {
            if (_container.Resolve<Management>().CurrentUser.userPart == Enums.UserPart.Operator)
            {
                IsAllowEdit = false;
            }
            else
            {
                IsAllowEdit = true;
            }
            try
            {
                SelectCalibration = _container.Resolve<CalibrationHomeViewModel>().SelectCalibration;
                management = _container.Resolve<Management>();
                robotService = management.RobotService;
                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("相机校准-校准验证界面示加载时出错!", ex);
                _eventAggregator.GetEvent<SnackbarMessageNotification>().Publish(new MessageParameter() { Msg = ex.Message, Duration = 1 });
            }
        }

        /// <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.验证完成);
                _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationResult");
            }
            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()
        {
            _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationResult");
        }

        /// <summary>
        /// 上一步
        /// </summary>
        void ExecuteBackCommand()
        {
            _regionManager.RequestNavigate("CalibrationRegionContext", "CalibrationAuto");
        }

        /// <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

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