Metis_xctg/AutoAnalysis/AutoFocusWorkflow.cs

using OpenCvSharp;
using OpenCvSharp.Extensions;
using PaintDotNet.Setting;
using StageController;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using System.Linq;
using System.Threading;

namespace Metis.AutoAnalysis
{
    public class AutoFocusWorkflow
    {
        private static bool _isWorking;
        public static double m_CurrentValue;
        public static int m_moveDirection = -1;//记录上次移动的方向
        public static void Stop()
        {
            _isWorking = false;
        }

        // Add by shayg 20220718 start
        private static int orientation = 1;

        /// <summary>
        /// 自动调焦
        /// </summary>
        /// <param name="axisController">载物台控制器</param>
        /// <param name="currentImage">当前图片</param>
        /// <param name="windowLength">趋势窗口长度</param>
        /// <param name="viewIndex">视场索引</param>
        /// <returns>最清晰图片对象</returns>
        public static (double Z, Bitmap Image) AutoFocusFastM3H(AxisController axisController, Func<Bitmap> currentImage, int windowLength, int viewIndex, string dirCurrent)
        {
            var focusBitmap = (0d, currentImage());
            if (_isWorking)
            {
                return focusBitmap;
            }

            int index = 0;
            int round = 0;
            var totalDistance = 0.0;
            var testValues = new List<(double Z, double Value, Bitmap Image)>();

            var stepLength = FocusingParameter.getRuleFocus().StepLength;
            var maxTotalDistance = Math.Min(stepLength * 100, 500);
            if (stepLength * windowLength > maxTotalDistance)
            {
                Logs.WriteFocus($"\t{viewIndex + 1}\t{index}\t{axisController.Z}\tStep Length is too large!(Step Length:{stepLength}, MaxTotalDistance:{maxTotalDistance}");
                return focusBitmap;
            }

            _isWorking = true;
            Thread.Sleep(100);
            axisController.WaitMoveDone();
            stepLength = stepLength * orientation;

            //var path = Path.Combine(dirCurrent, "FocusImages");
            //if (!Directory.Exists(path))
            //{
            //    Directory.CreateDirectory(path);
            //}

            while (_isWorking)
            {
                index = index + 1;
                totalDistance = totalDistance + stepLength;
                if (Math.Abs(totalDistance) >= maxTotalDistance)
                {
                    Logs.WriteFocus($"\t{viewIndex + 1}\t{index}\t{axisController.Z}\tOut of focus range!(TotalDistance:{totalDistance}, MaxTotalDistance:{maxTotalDistance})");
                    break;
                }

                var bitmap = currentImage();
                var meanValue = getMeanValueOfBitmap(bitmap);
                testValues.Add((axisController.Z, meanValue, bitmap));
                //SaveFocusImage(bitmap, path, viewIndex + 1, index, axisController.Z, meanValue);
                //Logs.WriteFocus($"\t{viewIndex + 1}\t{index}\t{axisController.Z}\t{meanValue}");

                if (IsChangeDir(testValues, windowLength))
                {
                    round = round + 1;
                    if (testValues.Count >= windowLength * 2 - 1 || round == 2)
                    {
                        orientation = stepLength > 0 ? -1 : 1;
                        var value = testValues[testValues.Count - windowLength];
                        focusBitmap = (value.Z, value.Image);
                        //Logs.WriteFocus($"\t{viewIndex + 1}\t - \t{value.Z}\t{value.Value}");
                        break;
                    }
                    else
                    {
                        stepLength = -stepLength;
                        ToUp(axisController, stepLength * testValues.Count);
                        testValues = testValues.Take(testValues.Count - windowLength + 1).OrderByDescending(v=>v.Value).ToList();
                    }
                }
                else
                {
                    ToUp(axisController, stepLength);
                }
            }

            _isWorking = false;
            return focusBitmap;
        }

        private static void SaveFocusImage(Bitmap img, string path, int viewIndex, int focusIndex, double z, double value)
        {
            var fileName = path + "\\" + string.Format($"{DateTime.Now.ToString("HHmmss")}_{viewIndex}_{focusIndex}_{z:f4}_{value:f6}.jpg");
            try
            {
                img.Save(fileName, System.Drawing.Imaging.ImageFormat.Jpeg);
            }
            catch (Exception ex)
            {
                LogHelper.log.Error("image save error", ex);
            }
        }

        private static bool IsChangeDir(List<(double Z, double Value, Bitmap Image)> values, int windowLength)
        {
            if (values != null && values.Count >= windowLength)
            {
                var window = values.Skip(values.Count - windowLength).ToList();
                var maxValues = window.OrderByDescending(v => v.Value).FirstOrDefault();
                return window.First().Value == maxValues.Value;
            }
            return false;
        }

        private static bool ToUp(AxisController axisController, double distance)
        {
            var upto = axisController.Z + distance;
            axisController.Up(distance);
            axisController.WaitMoveDone();

            while (Math.Abs(upto - axisController.Z) > 0.0625)
            {
                axisController.Up(upto - axisController.Z);
                axisController.WaitMoveDone();
                if (Math.Abs(upto - axisController.Z) > 0.0625)
                {
                    Logs.WriteFocus($"The z axis controller is not working properly(To:{upto}, Now:{axisController.Z})");
                    return false;
                }
            }
            return true;
        }
        // Add by shayg 20220718 end

        /// <summary>
        /// 自动对焦流程
        /// </summary>
        /// <param name="stage"></param>
        /// <param name="CurrentImage"></param>
        public static void AutoFocusFast(AxisController stage, Func<Bitmap> CurrentImage)
        {
            if (_isWorking)
                return;

            _isWorking = true;

            List<double> focusList = new List<double>();

            double pulse = FocusingParameter.getRuleFocus().StepLength / 4;//最小步长1.25um
            double currentValue = 0.0;
            //int dir = -1; //方向
            double trip = 0.0;
            // _mask = CreateMask(CurrentImage());
            try
            {
                while (_isWorking)
                {
                    //Console.Write("Before focus:");
                    stage.WaitMoveDone();
                    //Console.Write("自动聚焦:");
                    Thread.Sleep(40);
                    try
                    {
                        Bitmap m_bitmap = CurrentImage();
                        currentValue = getMeanValueOfBitmap(m_bitmap);
                        focusList.Add(currentValue);
                        //Console.WriteLine(string.Format("{0:f3}  Z:{1:f3}", m_CurrentValue, stage.Z));
                    }
                    catch (Exception ex)
                    {
                        return;
                    }

                    if (focusList.Count == 1) //这里是第一次运动
                    {
                        stage.Up(pulse * m_moveDirection);
                        continue;
                    }

                    //前两张确认调焦方向
                    if (focusList.Count == 2)
                    {
                        if (currentValue > focusList[focusList.Count - 2]) //判断是否跑对方向
                        {
                            stage.Up(pulse * m_moveDirection);//对的方向
                        }
                        else
                        {
                            m_moveDirection = -m_moveDirection; //相反方向
                            stage.Up(pulse * m_moveDirection);//这里不是跑回原来的位置，而是更过去一些，节约时间
                            Console.WriteLine("change direction");
                        }
                        continue;
                    }

                    if (currentValue > focusList[focusList.Count - 2])
                    {
                        //清晰度大于前一张图，继续同方向移动
                        trip += pulse * m_moveDirection;
                        if (Math.Abs(trip) > pulse * 10)
                        {
                            Console.WriteLine("Trip of Z out of autofocus range.");
                            return;
                        }
                        stage.Up(pulse * m_moveDirection);
                    }
                    else
                    {
                        //清晰度变小，返回峰值，对焦结束
                        int moveDir = -m_moveDirection; //相反方向
                        stage.Up(pulse * moveDir);//返回上一张图片
                        stage.WaitMoveDone();
                        return;
                    }
                }
            }
            catch
            {
            }
            finally
            {
                // Console.Write("End focus:");
                stage.WaitMoveDone();

                //m_CurrentValue = getMeanValueOfBitmap(CurrentImage());
                //Console.WriteLine(string.Format("聚焦完成:{0:f3}   Z:{1:f3}", m_CurrentValue, stage.Z));
                _isWorking = false;
                // stage.LockZ();
            }
        }

        //public static void AutoFocusFast(AxisController stage, Func<Bitmap> CurrentImage)
        //{
        //    if (_isWorking)
        //        return;

        //    _isWorking = true;

        //    int round = 0;//翻转次数
        //    double pulse = FocusingParameter.getRuleFocus().StepLength / 4;//最小步长1.25um
        //    double lastValue = 0.0;
        //    int dir = -1; //方向
        //    double trip = 0.0;
        //    // _mask = CreateMask(CurrentImage());
        //    try
        //    {
        //        while (_isWorking)
        //        {
        //            //Console.Write("Before focus:");
        //            stage.WaitMoveDone();
        //            //Console.Write("自动聚焦:");
        //            Thread.Sleep(40);
        //            try
        //            {
        //                Bitmap m_bitmap = CurrentImage();
        //                m_CurrentValue = getMeanValueOfBitmap(m_bitmap);
        //                //Console.WriteLine(string.Format("{0:f3}  Z:{1:f3}", m_CurrentValue, stage.Z));
        //            }
        //            catch (Exception ex)
        //            {
        //                return;
        //            }

        //            if (round > 1)
        //            {
        //                return;
        //            }
        //            else if (lastValue == 0)
        //            {
        //            }
        //            else if (m_CurrentValue > lastValue)
        //            {
        //                round = 1;
        //            }
        //            else
        //            {
        //                //Console.WriteLine("Reverse:" + (dir > 0 ? "+" : "-"));
        //                round++;
        //                dir = -dir;
        //            }
        //            lastValue = m_CurrentValue;

        //            trip += pulse * dir;
        //            if (Math.Abs(trip) > pulse * 10)
        //            {
        //                Console.WriteLine("Trip of Z out of autofocus range.");
        //                return;
        //            }
        //            stage.Up(pulse * dir);
        //        }
        //    }
        //    catch
        //    {
        //    }
        //    finally
        //    {
        //        // Console.Write("End focus:");
        //        stage.WaitMoveDone();

        //        //m_CurrentValue = getMeanValueOfBitmap(CurrentImage());
        //        //Console.WriteLine(string.Format("聚焦完成:{0:f3}   Z:{1:f3}", m_CurrentValue, stage.Z));
        //        _isWorking = false;
        //        // stage.LockZ();
        //    }
        //}

        static Mat _mask;
        private static Mat CreateMask(Bitmap img)
        {
            Mat converted = PaintDotNet.Camera.Tools.ToMat(img);
            Mat imageGrey = new Mat();
            try
            {
                if (converted.Channels() == 3)
                    OpenCvSharp.Cv2.CvtColor(converted, imageGrey, OpenCvSharp.ColorConversionCodes.RGB2GRAY);
                else if (converted.Channels() == 1)
                    imageGrey = converted;
            }
            catch (Exception)
            {
                imageGrey = converted;
            }
            //stage.SetSpeedZ(40);
            Mat mask = new Mat();
            Cv2.Threshold(imageGrey, mask, 0, 255, ThresholdTypes.Triangle);
            var kernel = Cv2.GetStructuringElement(MorphShapes.Rect, new OpenCvSharp.Size(20, 20));
            Cv2.Dilate(mask, mask, kernel);
            //Cv2.Erode(mask, mask, kernel);
            return mask;
        }
        private static Mat CreateMask(Mat imageGrey)
        {
            Mat mask = new Mat();
            Cv2.Threshold(imageGrey, mask, 0, 255, ThresholdTypes.Triangle);
            var kernel = Cv2.GetStructuringElement(MorphShapes.Rect, new OpenCvSharp.Size(20, 20));
            Cv2.Dilate(mask, mask, kernel);
            return mask;
        }

        /// <summary>
        /// 获取输入图片的清晰度
        /// </summary>
        /// <param name="bitmap"></param>
        /// <returns></returns>
        private static double getMeanValueOfBitmap(Bitmap bitmap)
        {
            // Modify by shayg 20220718 start
            //Mat converted = PaintDotNet.Camera.Tools.ToMat(bitmap);
            Mat converted = BitmapConverter.ToMat(bitmap);
            // Modify by shayg 20220718 end
            Mat imageGrey = new Mat();
            Mat imageSobel = new Mat();
            try
            {
                if (converted.Channels() == 3)
                    OpenCvSharp.Cv2.CvtColor(converted, imageGrey, OpenCvSharp.ColorConversionCodes.RGB2GRAY);
                else if (converted.Channels() == 1)
                    imageGrey = converted;
            }
            catch (Exception)
            {
                imageGrey = converted;
            }

            OpenCvSharp.Mat meanValueImage = new OpenCvSharp.Mat();
            OpenCvSharp.Mat meanStdValueImage = new OpenCvSharp.Mat();
            // Modify by shayg 20220718 start
            //_mask = CreateMask(imageGrey);
            ////求灰度图像的标准差  值越大越好 
            //OpenCvSharp.Cv2.MeanStdDev(imageGrey, meanValueImage, meanStdValueImage, _mask);
            Cv2.MeanStdDev(imageGrey, meanValueImage, meanStdValueImage);
            // Modify by shayg 20220718 end
            return meanStdValueImage.At<double>(0, 0);
        }

        // Add by shayg 20220718 start
        /// <summary>
        /// 获取输入图片的清晰度（拉普拉斯边缘清晰度判定）
        /// </summary>
        /// <param name="bitmap">原图片</param>
        /// <returns>清晰度（值越大越清晰）</returns>
        private static double getMeanValueOfBitmap2(Bitmap bitmap)
        {
            var converted = BitmapConverter.ToMat(bitmap);
            var imageGray = new Mat();
            if (converted.Channels() == 1)
            {
                imageGray = converted;
            }
            else
            {
                Cv2.CvtColor(converted, imageGray, ColorConversionCodes.RGB2GRAY);
            }

            var dst = new Mat();
            Cv2.Laplacian(imageGray, dst, MatType.CV_64F);

            Cv2.MeanStdDev(dst, out var mean, out var dev);
            var meanValue = dev.Val0 * dev.Val0;
            return meanValue;
        }
        // Add by shayg 20220718 end
    }
}