Health/Assets/Scripts/PoseCheck/MotionCaptureManager.cs

using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.UnityUtils;
using OpenCVForUnity.UnityUtils.Helper;

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
using UnityEngine.UI;

#if UNITY_ANDROID && !UNITY_EDITOR
using Serenegiant.UVC;
#endif

namespace Yoga
{
    public class MotionCaptureManager : CameraCaptureManagerBase
#if UNITY_ANDROID && !UNITY_EDITOR
        , IUVCDrawer
#endif
    {
        public WebCamTextureToMatHelper _webCamTextureToMatHelper;
        public float SamplingRate = 0.3f;
        public RawImage TargetDisplayer;

        private bool _isOnCamCapture = false;
        private Mat _bgrMat;
        private Texture2D _displayTexture;
        private bool _isCorrectAction = false;

        private Material _flipImg;

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

        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);
            EventManager.Instance.AddEventListener(YogaEventType.ChangeCaptureCameraDevice, ChangeCaptureCameraDevice);
        }

        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);
            EventManager.Instance.RemoveEventListener(YogaEventType.ChangeCaptureCameraDevice, ChangeCaptureCameraDevice);
        }

        public void Awake()
        {
            if (_webCamTextureToMatHelper == null)
                _webCamTextureToMatHelper = transform.GetComponent<WebCamTextureToMatHelper>();

#if UNITY_ANDROID && !UNITY_EDITOR
                // Avoids the front camera low light issue that occurs in only some Android devices (e.g. Google Pixel, Pixel2).
                _webCamTextureToMatHelper.avoidAndroidFrontCameraLowLightIssue = true;
#endif
            //tool loading
            _flipImg = Resources.Load<Material>("Materials/Unlit_FlipHorizontal");

            _webCamTextureToMatHelper.Initialize();
            Utils.setDebugMode(true); //打印日志
            GlobalData.Instance.CameraDeviceType = CameraDeviceType.USB; //test
#if UNITY_ANDROID && !UNITY_EDITOR
            var devices = UVCManager.Instance.GetAttachedDevices();
#endif
            CVEstimator.Instance.Init();//初始化姿态检测
        }

#if UNITY_ANDROID && !UNITY_EDITOR
        #region UVC
        public UVCFilter[] UVCFilters;
        private Texture SavedTexture;
        public bool OnUVCAttachEvent(UVCManager manager, UVCDevice device)
        {
            var result = !device.IsRicoh || device.IsTHETA;

            result &= UVCFilter.Match(device, UVCFilters);

            return result;
        }

        public void OnUVCDetachEvent(UVCManager manager, UVCDevice device)
        {
            LogPrint.Log("OnUVCDetachEvent " + device);
        }

        public bool CanDraw(UVCManager manager, UVCDevice device)
        {
            return UVCFilter.Match(device, UVCFilters);
        }

        public void OnUVCStartEvent(UVCManager manager, UVCDevice device, Texture tex)
        {
            HandleOnStartPreview(tex);
        }

        public void OnUVCStopEvent(UVCManager manager, UVCDevice device)
        {
            HandleOnStopPreview();
        }

        private void HandleOnStartPreview(Texture tex)
        {
            SavedTexture = TargetDisplayer.texture;
            TargetDisplayer.texture = _displayTexture;
        }
        private void HandleOnStopPreview()
        {
            RestoreTexture();
        }

        private void RestoreTexture()
        {
            try
            {
                transform.GetComponent<Renderer>().material.mainTexture = SavedTexture;
            }
            catch (Exception e)
            {
                Debug.LogException(e);
            }
            SavedTexture = null;
        }
        #endregion
#endif

        public Texture2D HorizontalFlipTexture(Texture2D texture)
        {
            //得到图片的宽高
            Texture2D retVal = new Texture2D(texture.width, texture.height);
            RenderTexture tmp = RenderTexture.GetTemporary(texture.width, texture.height, 0, RenderTextureFormat.ARGB32);
            Graphics.Blit(texture, tmp, _flipImg);
            RenderTexture.active = tmp;
            retVal.ReadPixels(new UnityEngine.Rect(0, 0, texture.width, texture.height), 0, 0);
            RenderTexture.ReleaseTemporary(tmp);
            return retVal;
        }

        private Mat GetMat()
        {
            Mat img = null;
#if UNITY_ANDROID && !UNITY_EDITOR
            if (GlobalData.Instance.CameraDeviceType == CameraDeviceType.USB &&
                UVCManager.Instance != null &&
                UVCManager.Instance.GetAttachedDevices() != null && UVCManager.Instance.GetAttachedDevices().Count > 0)
            {
                var devices = UVCManager.Instance.GetAttachedDevices();
                for (int i = 0; i < devices.Count; i++)
                {
                    UVCManager.CameraInfo device = devices[i];
                    LogPrint.Error($"id:{i},pid:{device.Pid},vid:{device.Vid},deviceName:{device.DeviceName}");
                }
                var formatRefMat = _webCamTextureToMatHelper.GetMat();
                var picCaptured = devices.FirstOrDefault().previewTexture;
                Texture2D tmpTex = new Texture2D(picCaptured.width, picCaptured.height);
                Utils.textureToTexture2D(picCaptured, tmpTex);
                tmpTex = HorizontalFlipTexture(tmpTex);//picCaptured texture水平反转
                img = new Mat(formatRefMat.rows(), formatRefMat.cols(), formatRefMat.type());
                Utils.texture2DToMat(tmpTex, img);
            }
            else
#endif
            {
                if (_webCamTextureToMatHelper.IsPlaying() && _webCamTextureToMatHelper.DidUpdateThisFrame())
                {
                    img = _webCamTextureToMatHelper.GetMat();
                    Imgproc.cvtColor(img, img, Imgproc.COLOR_BGR2RGB);
                }
            }

            return img;
        }

        private void Update()
        {
            if (!_isOnCamCapture)
            {
                return;
            }

            if (!transform.gameObject.activeSelf)
                transform.gameObject.SetActive(true);

            Mat img = GetMat();

            if (img == null)
                return;


            YogaManager.Instance.RgbaMat = img.clone();
            YogaManager.Instance.CurrentEstimator.Check(ref img);//检测模型，将错误信息打印在图片上

            if (YogaManager.Instance.VoloResult.Count >= 2)
                YogaManager.Instance.CurrentEstimator.DebugPrint(img);

            if (YogaManager.Instance.CurrPersonPoints != null && YogaManager.Instance.CurrPersonPoints.Count > 0)
            {
                List<Point> points = YogaManager.Instance.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);
                    }
                }
            }

            Utils.matToTexture2D(img, _displayTexture);
        }

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

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

        private void OnStopMotionCapture()
        {
            this.enabled = false;
            _isOnCamCapture = false;
        }
        private void GetActionBasePoint()
        {
            var startTime = DateTime.Now;
            while (true)
            {
                if (YogaManager.Instance.CurrPersonPoints != null && YogaManager.Instance.CurrPersonPoints.Count != 0)
                {
                    YogaManager.Instance.Points = YogaManager.Instance.CurrPersonPoints;
                    break;
                }

                if (startTime.AddMilliseconds(100) < DateTime.Now)
                {
                    LogPrint.Error("GetActionBasePoint timeout");
                    break;
                }
            }
        }

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


            //检测动作
            var startTime = DateTime.Now;

            while (true)
            {
                if (YogaManager.Instance.ActionCheckPoints(YogaManager.Instance.CurrPersonPoints))
                {
                    _isCorrectAction = (_isCorrectAction || YogaManager.Instance.IsCorrectAction(YogaManager.Instance.CurrPersonPoints, actionType));
                    break;
                }
                if (startTime.AddMilliseconds(100) < DateTime.Now)
                {
                    LogPrint.Warning("请摆正姿势");
                    LogPrint.Warning("EstimateAction timeout");
                    break;
                }
            }
        }

        private void ChangeCaptureCameraDevice()
        {
            var deviceName = WebCamTexture.devices[GlobalData.Instance.CameraIndex].name;
            var width = _webCamTextureToMatHelper.GetWidth();
            var height = _webCamTextureToMatHelper.GetHeight();
            var fps = _webCamTextureToMatHelper.GetFPS();
            if (fps > 0)
            {
                _webCamTextureToMatHelper.Initialize(deviceName, width, height, requestedFPS: fps);
            }
            else
            {
                _webCamTextureToMatHelper.Initialize(deviceName, width, height);
            }
        }

        private void ChangeToUSBCaptureCamera()
        {

        }
        #endregion



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

            YogaManager.Instance.CurrentEstimator.Dispose();

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

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

            Mat webCamTextureMat = _webCamTextureToMatHelper.GetMat();

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

            //gameObject.GetComponent<Renderer>().material.mainTexture = texture;
            TargetDisplayer.texture = _displayTexture;

            gameObject.transform.localScale = new Vector3(webCamTextureMat.cols() / 10, webCamTextureMat.rows() / 10, 1);
            LogPrint.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()
        {
            LogPrint.Log("OnWebCamTextureToMatHelperDisposed");

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

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

        public void OnWebCamTextureToMatHelperErrorOccurred(WebCamTextureToMatHelper.ErrorCode errorCode)
        {
            LogPrint.Log("OnWebCamTextureToMatHelperErrorOccurred " + errorCode);
        }

        public void ScoreUpdate()
        {

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

            _isCorrectAction = false;//重置
        }
    }

    public abstract class CameraCaptureManagerBase : MonoBehaviour
    {
    }
}