Health/Assets/Scripts/PoseCheck/MotionCaptureManager.cs

#undef DEBUG_MODE
#define DEBUG_MODE


using OpenCVForUnity.CoreModule;
using OpenCVForUnity.DnnModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.ObjdetectModule;
using OpenCVForUnity.UnityUtils;
using OpenCVForUnity.UnityUtils.Helper;
using OpenCVForUnityExample;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using UnityEngine;
using UnityEngine.Video;

namespace Yoga
{
    [RequireComponent(typeof(WebCamTextureToMatHelper))]
    public class MotionCaptureManager : MonoSingleton<MotionCaptureManager>
    {
        private WebCamTextureToMatHelper _webCamTextureToMatHelper;
        private bool isInited = false;
        private Texture2D texture;

        private Net _net;
        private Dictionary<ModelType, KeypointsModel> _models = new Dictionary<ModelType, KeypointsModel>();

        private KeypointsModel _openPoseModel;

        public int inpWidth = 416;
        public int inpHeight = 416;
        public float confThreshold = 0.35f;
        public float nmsThreshold = 0.6f;
        public int topK = 1000;

        public float SamplingRate = 0.3f;

        private List<float[]> _voloResult = new List<float[]>();
        private List<Point> _currPersonPoints = new List<Point>();

        private YOLOv7ObjectDetector _objectDetector;
        private Mat _bgrMat;

        private double threshold = 0.5;

        private bool _isOnCamCapture = false;
        public bool IsOnCamCapture { get => _isOnCamCapture; internal set => _isOnCamCapture = value; }
        public WebCamTextureToMatHelper WebCamTextureToMatHelper => _webCamTextureToMatHelper;

        public List<Point> CurrPersonPoints { get => _currPersonPoints; set => _currPersonPoints = value; }
        public List<float[]> VoloResult { get => _voloResult; set => _voloResult = value; }
        private bool _isCorrectAction = false;
        private Mat _rgbaMat;
        public Mat RgbaMat { get => _rgbaMat; set => _rgbaMat = value; }

        private void OnEnable()
        {
            EventManager.Instance.AddEventListener(YogaEventType.StartMotionCapture, OnStartMotionCapture);
            EventManager.Instance.AddEventListener(YogaEventType.StopMotionCapture, OnStopMotionCapture);
            EventManager.Instance.AddEventListener(YogaEventType.EstimateAction, EstimateAction);
            EventManager.Instance.AddEventListener(YogaEventType.ScoreUpdate, ScoreUpdate);
            EventManager.Instance.AddEventListener(YogaEventType.GetActionBasePoint, GetActionBasePoint);
        }

        private void OnDisable()
        {
            EventManager.Instance.RemoveEventListener(YogaEventType.StartMotionCapture, OnStartMotionCapture);
            EventManager.Instance.RemoveEventListener(YogaEventType.StopMotionCapture, OnStopMotionCapture);
            EventManager.Instance.RemoveEventListener(YogaEventType.EstimateAction, EstimateAction);
            EventManager.Instance.RemoveEventListener(YogaEventType.ScoreUpdate, ScoreUpdate);
            EventManager.Instance.RemoveEventListener(YogaEventType.GetActionBasePoint, GetActionBasePoint);
        }

        public override void Init()
        {
            base.Init();

            _webCamTextureToMatHelper = gameObject.GetComponent<WebCamTextureToMatHelper>();
            _webCamTextureToMatHelper.Initialize();

            YogaManager.Instance.InitData();

            //动作引导界面
            var video = Resources.Load<VideoClip>(YogaManager.Instance.Action.VideoPath);

            UIManager.Instance.Init();
            UIManager.Instance.ShowPanel<ActionGuideVideoPanel>(false, video);

            LoadModels();//加载模型

            CVEstimator.Instance.Init(_objectDetector, _openPoseModel);//初始化姿态检测
        }

        private void LoadModels()
        {
            Utils.setDebugMode(true); //打印日志
            _net = null;

            //先用YOLOv7检测人体，再用OpenPose检测人体姿态
            _objectDetector = new YOLOv7ObjectDetector(
                Utils.getFilePath("OpenCVForUnity/dnn/yolov7-tiny.weights"),
                Utils.getFilePath("OpenCVForUnity/dnn/yolov7-tiny.cfg"),
                Utils.getFilePath("OpenCVForUnity/dnn/coco.names"),
                new Size(inpWidth, inpHeight), confThreshold, nmsThreshold/*, topK*/);


            var modelFilePath = Utils.getFilePath(YogaConfig.MODEL_PATHS[ModelType.OpenPose]);
            if (string.IsNullOrEmpty(modelFilePath))
            {
                Debug.LogError("modelFilePath is empty. Please copy from “OpenCVForUnity/StreamingAssets/” to “Assets/StreamingAssets/” folder. ");
                return;
            }

            _net = Dnn.readNet(modelFilePath);

            _openPoseModel = new KeypointsModel(_net);
            _openPoseModel.setInputScale(new Scalar(YogaConfig.InScale));
            _openPoseModel.setInputSize(new Size(YogaConfig.InWidth, YogaConfig.InHeight));
            _openPoseModel.setInputMean(new Scalar(YogaConfig.InMean));
            _openPoseModel.setInputSwapRB(false);
            _openPoseModel.setInputCrop(false);
        }

        private void Update()
        {
            if (!_isOnCamCapture)
            {
                return;
            }
            if (!transform.gameObject.activeSelf)
                transform.gameObject.SetActive(true);

            if (_webCamTextureToMatHelper.IsPlaying() && _webCamTextureToMatHelper.DidUpdateThisFrame())
            {
                Mat img = _webCamTextureToMatHelper.GetMat();
                Imgproc.cvtColor(img, img, Imgproc.COLOR_BGR2RGB);
                _rgbaMat = img.clone();
                if (_net == null)
                {
                    Imgproc.putText(img, "model file is not loaded.", new Point(5, img.rows() - 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255), 2, Imgproc.LINE_AA, false);
                    Imgproc.putText(img, "Please read console message.", new Point(5, img.rows() - 10), Imgproc.FONT_HERSHEY_SIMPLEX, 0.7, new Scalar(255, 255, 255), 2, Imgproc.LINE_AA, false);
                }

                if (_voloResult.Count >= 2)
                    DebugPrintObjectLayout(img, _voloResult[0], _voloResult[1], _voloResult[2]);

                if (_currPersonPoints != null && _currPersonPoints.Count > 0)
                {
                    List<Point> points = _currPersonPoints;

                    for (int i = 0; i < YogaConfig.POSE_PAIRS.GetLength(0); i++)
                    {
                        string partFrom = YogaConfig.POSE_PAIRS[i, 0];
                        string partTo = YogaConfig.POSE_PAIRS[i, 1];

                        int idFrom = YogaConfig.BODY_PARTS[partFrom];
                        int idTo = YogaConfig.BODY_PARTS[partTo];

                        if (points[idFrom] == new Point(-1, -1) || points[idTo] == new Point(-1, -1))
                            continue;

                        if (points[idFrom] != null && points[idTo] != null)
                        {
                            Imgproc.line(img, points[idFrom], points[idTo], new Scalar(0, 255, 0), 3);
                            Imgproc.ellipse(img, points[idFrom], new Size(3, 3), 0, 0, 360, new Scalar(0, 0, 255), Core.FILLED);
                            Imgproc.ellipse(img, points[idTo], new Size(3, 3), 0, 0, 360, new Scalar(0, 0, 255), Core.FILLED);
                        }
                    }
                }

                //#endif
                Utils.matToTexture2D(img, texture);
            }
        }

        #region Event Func
        private void OnStartMotionCapture()
        {
            this.enabled = true;
            _isOnCamCapture = true;

            CVEstimator.Instance.StartEstimation();//开始姿态检测
        }

        private void OnStopMotionCapture()
        {
            this.enabled = false;
            _isOnCamCapture = false;
        }
        private async void GetActionBasePoint()
        {
            YogaManager.Instance.Points = _currPersonPoints;
            //await Task.Run(() =>
            //{
            //    int i = 0;
            //    //确保捕捉到该次检测所需所有的点位
            //    while (true)
            //    {
            //        YogaManager.Instance.Points = GetEstimationBodyPoints();
            //        _currPersonPoints = YogaManager.Instance.Points;
            //        if (YogaManager.Instance.ActionCheckPoints(YogaManager.Instance.Points))
            //            break;
            //        i++;
            //        Debug.LogWarning($"执行{i}次");
            //    }
            //});
        }

        private async void EstimateAction(params object[] args)
        {
            var type = args.FirstOrDefault();
            if (type == null)
            {
                Debug.LogError("EstimateAction type is null");
                return;
            }
            AvatarAction actionType = (AvatarAction)args.FirstOrDefault();

            //await Task.Run(() =>
            //{
            //    _personPoints?.Clear();
            //    int i = 0;
            //    while (true)
            //    {
            //        //动作检测                    
            //        _personPoints = GetEstimationBodyPoints();
            //        if (YogaManager.Instance.ActionCheckPoints(_personPoints))
            //            break;
            //        i++;
            //        Debug.LogWarning($"执行{i}次");
            //    }
            //});

            //if (_personPoints == null || _personPoints.Count == 0)
            //{
            //    Debug.LogWarning("EstimateAction _personPoints is null");
            //    return;
            //}

            //检测动作
            _isCorrectAction = (_isCorrectAction || YogaManager.Instance.IsCorrectAction(_currPersonPoints, actionType));
        }

        #endregion
        private List<Point> GetEstimationBodyPoints()
        {
            Mat rgbaMat = _rgbaMat;
            Mat bgrMat = new Mat();

            if (rgbaMat == null)
            {
                Debug.LogWarning("WebCamTexture is null. ");
                return null;
            }

            if (_objectDetector == null)
            {
                Debug.LogWarning("ObjectDetector is not ready. ");
                return null;
            }
            Imgproc.cvtColor(rgbaMat, bgrMat, Imgproc.COLOR_RGBA2BGR);
            Mat results = _objectDetector.infer(bgrMat);
            var voloResultBox = new List<float[]>();

            bool hasValidObject = false;
            for (int i = results.rows() - 1; i >= 0; --i)
            {
                float[] box = new float[4];
                results.get(i, 0, box); //方框
                float[] conf = new float[1];
                results.get(i, 4, conf); //检测数据
                float[] cls = new float[1];
                results.get(i, 5, cls); //类别

                if (!IsObjectValid(box, conf, cls, rgbaMat))
                {
                    continue;
                }
                hasValidObject = true;
                voloResultBox.Clear();
                voloResultBox.Add(box);
                voloResultBox.Add(conf);
                voloResultBox.Add(cls);
                break;
            }

            if (!hasValidObject) //没有检测到人体
                return null;

            //对人体进行姿态检测
            _voloResult = voloResultBox;

            OpenCVForUnity.CoreModule.Rect roiRect = new OpenCVForUnity.CoreModule.Rect(
                (int)voloResultBox[0][0],
                (int)voloResultBox[0][1],
                Math.Abs((int)(voloResultBox[0][2] - voloResultBox[0][0])),
                Math.Abs((int)(voloResultBox[0][3] - voloResultBox[0][1])));

            if (roiRect.y < 0 || //0 <= _rowRange.start
                (roiRect.y + roiRect.height) < roiRect.y || // _rowRange.start <= _rowRange.end
                bgrMat.rows() < (roiRect.y + roiRect.height)) //_rowRange.end <= m.rows 
                return null;

            List<Point> points = null;
            try
            {
                Mat personRectImg = new Mat(bgrMat, roiRect);//获取人体区域
                points = _openPoseModel.estimate(personRectImg, (float)threshold).toList();
                //将人体区域的坐标转换为原图坐标
                for (int j = 0; j < points.Count; j++)
                {
                    if (points[j] == null ||
                        (points[j].x == -1 && points[j].y == -1)) //没找到的点，跳过
                        continue;

                    points[j].x += roiRect.x;
                    points[j].y += roiRect.y;
                }
            }
            catch (Exception e)
            {
                Debug.LogException(e);
                Debug.LogWarning("bgrMat:" + bgrMat.rows());
                Debug.LogError("rect:" + roiRect);
            }

            bgrMat.Dispose();
            return points;
        }

        private bool IsObjectValid(float[] box, float[] confidence, float[] classID, Mat rgbaMat)
        {
            if ((int)classID[0] != 0 || confidence[0] < 0.8f) //只检测人体，且置信度大于80%
                return false;

            //是否满足主驾/副驾的位置条件
            float width = rgbaMat.width();
            //获取矩形的中点
            float centerX = (box[0] + box[2]) / 2;
            //左边副驾驶，右边主驾驶


            return true;
        }

        private void DebugPrintObjectLayout(Mat image, float[] box, float[] conf, float[] cls, bool isRGB = false)
        {

            float left = box[0];
            float top = box[1];
            float right = box[2];
            float bottom = box[3];
            int classId = (int)cls[0];

            Scalar c = _objectDetector.palette[classId % _objectDetector.palette.Count];
            Scalar color = isRGB ? c : new Scalar(c.val[2], c.val[1], c.val[0], c.val[3]);

            Imgproc.rectangle(image, new Point(left, top), new Point(right, bottom), color, 2);

            string label = String.Format("{0:0.00}", conf[0]);
            if (_objectDetector.classNames != null && _objectDetector.classNames.Count != 0)
            {
                if (classId < (int)_objectDetector.classNames.Count)
                {
                    label = _objectDetector.classNames[classId] + " " + label;
                }
            }

            int[] baseLine = new int[1];
            Size labelSize = Imgproc.getTextSize(label, Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, 1, baseLine);

            top = Mathf.Max((float)top, (float)labelSize.height);
            Imgproc.rectangle(image, new Point(left, top - labelSize.height),
                new Point(left + labelSize.width, top + baseLine[0]), color, Core.FILLED);
            Imgproc.putText(image, label, new Point(left, top), Imgproc.FONT_HERSHEY_SIMPLEX, 0.5, Scalar.all(255), 1, Imgproc.LINE_AA);
        }

        /// <summary>
        /// Raises the destroy event.
        /// </summary>
        void OnDestroy()
        {
            _webCamTextureToMatHelper.Dispose();

            if (_net != null)
                _net.Dispose();

            _bgrMat.Dispose();
            _models.Clear();
            CVEstimator.Instance.Dispose();
        }

        public void OnWebCamTextureToMatHelperInitialized()
        {
            Debug.Log("OnWebCamTextureToMatHelperInitialized");

            Mat webCamTextureMat = _webCamTextureToMatHelper.GetMat();

            texture = new Texture2D(webCamTextureMat.cols(), webCamTextureMat.rows(), TextureFormat.RGB24, false);
            Utils.matToTexture2D(webCamTextureMat, texture);

            this.gameObject.GetComponent<Renderer>().material.mainTexture = texture;

            gameObject.transform.localScale = new Vector3(webCamTextureMat.cols() / 10, webCamTextureMat.rows() / 10, 1);
            Debug.Log("Screen.width " + Screen.width + " Screen.height " + Screen.height + " Screen.orientation " + Screen.orientation);

            float width = webCamTextureMat.width();
            float height = webCamTextureMat.height();

            float widthScale = (float)Screen.width / width;
            float heightScale = (float)Screen.height / height;
            if (widthScale < heightScale)
            {
                Camera.main.orthographicSize = (width * (float)Screen.height / (float)Screen.width) / 2;
            }
            else
            {
                Camera.main.orthographicSize = height / 2;
            }

            _bgrMat = new Mat(webCamTextureMat.rows(), webCamTextureMat.cols(), CvType.CV_8UC3);
        }

        //event call
        public void OnWebCamTextureToMatHelperDisposed()
        {
            Debug.Log("OnWebCamTextureToMatHelperDisposed");

            if (_bgrMat != null)
                _bgrMat.Dispose();

            if (texture != null)
            {
                Texture2D.Destroy(texture);
                texture = null;
            }
        }

        public void ScoreUpdate()
        {

            if (_isCorrectAction)
            {
                EventManager.Instance.Dispatch(YogaEventType.ActionSuccess);
            }
            else
            {
                EventManager.Instance.Dispatch(YogaEventType.ActionFailed);
            }

            _isCorrectAction = false;//重置
        }
    }
}