RUS/SourceCode/OnGoingTasks/VisionRoughPositioning/Demo/HumanSurfaceOrganAnalyser/HumanSurfaceOrganAnalyser.cs

using AI.Common;
using AI.Common.Log;
using HumanOrganSegmentDemo;
using RUSInferNet.Vision;
using RUSInferNet;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using HumanOrganSegDemo.HumanBodyDetector;
using HumanOrganSegDemo.HumanBodyPartAnalyser.OrganSegment;
using HumanOrganSegDemo.HumanSurfaceOrganAnalyser.HumanPoseEstimate;
using System.Windows.Media;
using RUSInferNet.PostProcess;

namespace HumanOrganSegDemo.HumanBodyPartAnalyser
{
    public class HumanSurfaceOrganAnalyser : IHumanSurfaceOrganAnalyser
    {
        #region private

        private IHumanDetector _humanDetector;
        private IHumanPoseEstimate _humanPoseEstimate;
        private IHumanSurfaceOrganSegment _surfaceOrganSeg;

        /// <summary>
        /// 像素坐标系下人体朝向的方向向量
        /// </summary>
        public Point2D _humanOrientationInPCS;
        // 是否完成了人体朝向分析
        public volatile bool _orientationFinish = false;

        public Dictionary<EnumHumanParts, Rect[]> _bodyPartBoundBoxesAll = new Dictionary<EnumHumanParts, Rect[]>();
        public BodyKeyPoints _bodyKeyPoints;
        public ContourPoints _organContours;

        /// <summary>
        /// 是否启用推理
        /// </summary>
        private volatile bool _enable = false;

        private ConcurrentQueue<RawImage> _inputImages = new ConcurrentQueue<RawImage>();
        private readonly ManualResetEvent _waitImageEvent = new ManualResetEvent(true);
        private readonly ManualResetEvent _processFinishEvent = new ManualResetEvent(true);
        private Thread _imageProcessThread;
        private volatile bool _disposing = false;

        private const int _queueMaxSize = 1;

        #endregion

        #region 实现

        /// <summary>
        /// 是否启用推理
        /// </summary>
        /// <returns></returns>
        public bool Enable
        {
            get { return _enable; }
            set { _enable = value; }
        }

        public void EvaluateOneImage(RawImage image)
        {
            if (_enable)
            {
                _inputImages.Enqueue(image.Clone());
                _waitImageEvent.Set();
                if (_imageProcessThread == null || !_imageProcessThread.IsAlive)
                {
                    _imageProcessThread = new Thread(() => DoImageProcess())
                    {
                        IsBackground = true,
                        Name = "HumanSurfaceOrganAnalyser_Process"
                    };
                    _imageProcessThread.Start();
                }
            }
        }

        /// <summary>
        /// 通知订阅者，人体检测有结果更新
        /// </summary>
        public event EventHandler<HumanDetectResultPerImage> NotifyHumanDetectFinish;

        /// <summary>
        /// 通知订阅者，推理过程中发生了错误
        /// </summary>
        public event EventHandler<ErrorEventArgs> NotifyError;

        /// <summary>
        /// 通知订阅者，有log要记
        /// </summary>
        public event EventHandler<LogEventArgs> NotifyLogWrite;

        /// <summary>
        /// 销毁
        /// </summary>
        public void Dispose()
        {
            DoDispose();
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// 析构函数
        /// </summary>
        ~HumanSurfaceOrganAnalyser()
        {
            DoDispose();
        }

        #endregion

        #region constructor

        public HumanSurfaceOrganAnalyser(EnumOrgans organName,int numCpu, string netDir)
        {
            _humanDetector = new HumanBodyPartDetector(numCpu, netDir);
            _humanPoseEstimate = new HumanPoseEstimate(numCpu, netDir);

            switch (organName)
            {
                case EnumOrgans.Heart:
                    _surfaceOrganSeg = new HumanSurfaceHeartSegment(numCpu, netDir);
                    break;
                case EnumOrgans.Liver:
                    _surfaceOrganSeg = new HumanSurfaceLiverSegment(numCpu, netDir);
                    break;
                default:
                    throw new ArgumentException("Invalid Organ name!");
            }

            _humanDetector.NotifyLogWrite += OnLogWrite;
            _humanDetector.NotifyError += OnErrorOccur;

            _humanPoseEstimate.NotifyLogWrite += OnLogWrite;
            _humanPoseEstimate.NotifyError += OnErrorOccur;

            _surfaceOrganSeg.NotifyLogWrite += OnLogWrite;
            _surfaceOrganSeg.NotifyError += OnErrorOccur;
        }

        #endregion

        #region private funcs

        private void DoImageProcess()
        {
            while (!_disposing)
            {
                if (_inputImages.Count > 0)
                {
                    _inputImages.TryDequeue(out RawImage image);

                    // 如果队列里待处理的数据过多，则对该图不做处理，直接跳到下一幅图
                    if (_inputImages.Count >= _queueMaxSize)
                    {
                        image.Dispose();
                        continue;
                    }

                    try
                    {
                        // 让dispose的线程等待执行完毕后再销毁
                        _processFinishEvent.Reset();
                        var totalStartTime = Environment.TickCount;

                        var imageWidth = image.Width;
                        var imageHeight = image.Height;

                        //清空历史数据
                        if (_bodyPartBoundBoxesAll != null)
                        {
                            _bodyPartBoundBoxesAll.Clear();
                        }
                        _humanOrientationInPCS = new Point2D(0, 0);
                        _bodyKeyPoints = new BodyKeyPoints();
                        _organContours = new ContourPoints();
                        Rect humanRc = new Rect();
                        Rect faceRc=new Rect();
                        // 目标检测（人体和人脸）
                        var detectedHumans = _humanDetector.EvaluateOneImage(image);
                        // 对检测出的人体框（可能是多个框）进行分析，决策出一个输出框作为目标
                        HumanDetectResultProcess(detectedHumans, out humanRc,out faceRc);
                        //检测出了有效人体框
                        if (humanRc.Width * humanRc.Height >0)
                        {
                            // 对ROI区域内的人体进行体表器官分割
                            var result = _surfaceOrganSeg.EvaluateOneImage(image, humanRc);
                            //依据人脸（假如有）分析人体朝向，没有则依据轮廓与关键点计算人体朝向向量
                            EvaluateHumanOrientationInPCS(image,humanRc, faceRc,result);
                        }
                        
                        // 用过后的图像销毁
                        image.Dispose();

                        // 执行完毕了，dispose的线程可以开始销毁了
                        _processFinishEvent.Set();
                        //运行耗时
                        var totalTime = Environment.TickCount - totalStartTime;
                        int timeElapsed = totalTime;

                        // 通知订阅者，预处理的结果有更新
                        NotifyHumanDetectFinish?.Invoke(this, new HumanDetectResultPerImage(_bodyPartBoundBoxesAll, humanRc, _bodyKeyPoints, _humanOrientationInPCS, _organContours, timeElapsed));
                    }
                    catch (Exception excep)
                    {
                        _processFinishEvent.Set();  
                        NotifyError?.Invoke(this, new ErrorEventArgs(excep));
                    }
                }
                else
                {
                    // 如果已经没有要处理的数据了，就等有新数据输入的时候再继续while循环
                    _waitImageEvent.Reset();
                    _waitImageEvent.WaitOne();
                }
                Thread.Sleep(1);
            }
        }

        /// <summary>
        /// 人体检测结果处理 todo
        /// 1：提取人体框（作为分割模块的ROI）
        /// 2：提取人脸框
        /// </summary>
        /// <param name="humans"></param>
        /// <returns></returns>
        /// <exception cref="ArgumentException"></exception>
        private void HumanDetectResultProcess(IDetectedObject[] humans, out Rect HumanRect,out Rect FaceRect)
        {
            _orientationFinish = false;
            List<Rect> humanRect = new List<Rect>();
            List<Rect> faceRect = new List<Rect>();
            HumanRect=new Rect();
            FaceRect=new Rect();
            List<float> humanConfidence = new List<float>();
            for (int i =0; i< humans.Length; ++i)
            {
                switch (humans[i].Label)
                {
                    case 1:
                        faceRect.Add(humans[i].BoundingBox);
                        break;
                    case 2:
                        humanRect.Add(humans[i].BoundingBox);
                        humanConfidence.Add(humans[i].Confidence);
                        break;
                    default:
                        throw new ArgumentException("Invalid humans part label!");
                }
            }
            _bodyPartBoundBoxesAll.Add(EnumHumanParts.Head, faceRect.ToArray());
            _bodyPartBoundBoxesAll.Add(EnumHumanParts.Body, humanRect.ToArray());

            // todo 一幅图里出现多个人或者多张脸，需要用做处理

            // 临时实现方案
            //出现多个人，取置信度最高的框
            List<float> temp = new List<float>();

            if (humanRect.Count > 0)
            {
                for (int i = 0; i < humanRect.Count; i++)
                {
                    temp.Add(humanConfidence[i]);
                }
                temp.Sort();
                for (int j = 0; j < temp.Count; j++)
                {
                    if (humanConfidence[j] == temp[temp.Count - 1])
                    {
                        HumanRect = humanRect[j];
                    }
                }

                // 只出现一张脸时返回结果框
                if (faceRect.Count == 1)
                {
                    FaceRect=faceRect[0];
                }
            }
        }

        /// <summary>
        /// 得到轮廓并计算人体朝向
        /// </summary>
        /// <param name="image"></param>
        /// <param name="HumanRoi"></param>
        /// <param name="ContoursResult"></param>
        private void EvaluateHumanOrientationInPCS(RawImage image,Rect HumanRoi, Rect FaceRoi,IDetectedObject[] ContoursResult)
        {
            int midX, midY;
            Point2D bodyKeyPtsPoint = new Point2D(0, 0);
            Point2D humanCenter = new Point2D(0, 0);
            Point2D faceCenter = new Point2D(0, 0);
            //依据人脸框和人体框给出人体朝向向量
            if (FaceRoi.Width* FaceRoi.Height>0)
            {
                humanCenter = new Point2D(HumanRoi.Left + HumanRoi.Width / 2, HumanRoi.Top + HumanRoi.Height / 2);
                faceCenter = new Point2D(FaceRoi.Left + FaceRoi.Width / 2, FaceRoi.Top + FaceRoi.Height / 2);
                _humanOrientationInPCS = new Point2D(faceCenter.X - humanCenter.X, faceCenter.Y - humanCenter.Y);
                _orientationFinish = true;
            }
            //得到轮廓点
            if (ContoursResult.Length > 0 && ContoursResult[0].Contour != null && ContoursResult[0].Contour.Contours.Length > 0)
            {
                _organContours = ContoursResult[0].Contour.Contours[0];
                int len = ContoursResult[0].Contour.Contours[0].Points.Length;
                //轮廓中心点
                List<int> contoursX = new List<int>();
                List<int> contoursY = new List<int>();
                for (int i = 0; i < len; i++)
                {
                    contoursX.Add(ContoursResult[0].Contour.Contours[0].Points[i].X);
                    contoursY.Add(ContoursResult[0].Contour.Contours[0].Points[i].Y);
                }
                contoursX.Sort();
                contoursY.Sort();
                midX = (contoursX[0] + contoursX[len - 1]) / 2;
                midY = (contoursY[0] + contoursY[len - 1]) / 2;
            }
            //人体框中心点
            else
            {
                midX = HumanRoi.Left + HumanRoi.Width / 2;
                midY = HumanRoi.Top + HumanRoi.Height / 2;
            }
            // 如果没有分析出人体朝向，则调用关键点检测模型进行关节提取并分析人体朝向
            if (!_orientationFinish)
            {
                var bodyKeyPts = _humanPoseEstimate.EvaluateOneImage(image, HumanRoi);
                List<float> temp = new List<float>();
                // todo 已经限制了roi,最多检测出一个人，如果检测不到或者检测多个人的关节点，都不做处理
                if (bodyKeyPts.Length > 0)
                {
                    for (int i = 0; i < bodyKeyPts.Length; i++)
                    {
                        temp.Add(bodyKeyPts[i].Confidence);
                    }
                    //选取置信度高的人,如果不包含上半身关键点则找次高置信度，依此类推
                    temp.Sort();
                    for (int i = 0; i < bodyKeyPts.Length; i++)
                    {
                        for (int j = 0; j < temp.Count; j++)
                        {
                            if (bodyKeyPts[j].Confidence == temp[temp.Count - i - 1])
                            {
                                bodyKeyPtsPoint = HumanKeyPtsProcess((DetectedHuman)bodyKeyPts[j]);
                                if (bodyKeyPtsPoint.X != 0 || bodyKeyPtsPoint.Y != 0)
                                {
                                    i = bodyKeyPts.Length;
                                    break;
                                }
                            }
                        }
                    }
                    _humanOrientationInPCS = new Point2D(bodyKeyPtsPoint.X - midX, bodyKeyPtsPoint.Y - midY);
                }
            }
        }


        /// <summary>
        /// 依据关键点进行人体朝向分析 todo
        /// </summary>
        /// <param name="humansKeyPts"></param>
        /// <returns></returns>
        private Point2D HumanKeyPtsProcess(DetectedHuman humansKeyPts)
        {
            // todo
            _bodyKeyPoints = humansKeyPts.KeyPoints;

            if (_bodyKeyPoints.Neck.X>0)
            {
                return _bodyKeyPoints.Neck;
            }
            else if (_bodyKeyPoints.LeftShoulder.X > 0)
            {
                return _bodyKeyPoints.LeftShoulder;
            }
            else if (_bodyKeyPoints.RightShoulder.X > 0)
            {
                return _bodyKeyPoints.RightShoulder;
            }
            else
            {
                return new Point2D(0, 0);
            }
        }

        /// <summary>
        /// 主动销毁
        /// </summary>
        private void DoDispose()
        {
            if (!_disposing)
            {
                _disposing = true;
                _waitImageEvent.Set();
                _processFinishEvent.WaitOne();
                if (_humanDetector != null)
                {
                    _humanDetector.Dispose();
                    _humanDetector.NotifyLogWrite -= OnLogWrite;
                    _humanDetector.NotifyError -= OnErrorOccur;
                    _humanDetector = null;
                }
                if (_humanPoseEstimate != null)
                {
                    _humanPoseEstimate.Dispose();
                    _humanPoseEstimate.NotifyLogWrite -= OnLogWrite;
                    _humanPoseEstimate.NotifyError -= OnErrorOccur;
                    _humanPoseEstimate = null;
                }
                if (_surfaceOrganSeg != null)
                {
                    _surfaceOrganSeg.Dispose();
                    _surfaceOrganSeg.NotifyLogWrite -= OnLogWrite;
                    _surfaceOrganSeg.NotifyError -= OnErrorOccur;
                    _surfaceOrganSeg = null;
                }

                while (_inputImages.Count > 0)
                {
                    if (_inputImages.TryDequeue(out var input))
                    {
                        input?.Dispose();
                    }
                }

            }
        }

        /// <summary>
        /// 有log要记
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void OnLogWrite(object sender, LogEventArgs e)
        {
            NotifyLogWrite?.Invoke(sender, e);
        }

        /// <summary>
        /// 有错误发生
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void OnErrorOccur(object sender, ErrorEventArgs e)
        {
            NotifyError?.Invoke(sender, e);
        }

        #endregion
    }
}