GHZSDKXR/Runtime/Tracking3D/Scripts/Tracking3DManager.cs

using EZXR.Glass;
using EZXR.Glass.Core;
using EZXR.Glass.Device;
using EZXR.Glass.SixDof;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using UnityEngine;

namespace EZXR.Glass.Tracking3D
{
    public class Tracking3DManager : MonoBehaviour
    {

        public bool IsLocationSuccess { get { return isLocationSuccess; } }

        private bool isLocationSuccess;
        public Pose AnchorPose
        {
            get
            {
                return _anchorPose;
            }
        }
        private Pose _anchorPose;


        private EZXRTrack3dSession Track3dsession;
        private NormalRGBCameraDevice rgbCameraDevice;
        private EZVIOInputImage locCamImageBuffer;
        private float[] locCamIntriarray = new float[8];
        private OSTMatrices ost_m = new OSTMatrices();
        private bool hasIntricsSet = false;
        private bool hasPoseOffsetSet = false;

        private string algAssetPath = "";

        // Start is called before the first frame update
        void Start()
        {

        }
        private void OnEnable()
        {
            StartCoroutine(startRGBCamera());
        }
        private void OnDisable()
        {
            stopRGBCamera();
            stopTrackSession();
        }
        // Update is called once per frame
        void Update()
        {

        }

        public void enableTracking(string path)
        {
            algAssetPath = path;
            stopTrackSession();
            StartCoroutine(startTrackSession());
        }
        private IEnumerator startRGBCamera()
        {
            yield return new WaitUntil(() => SessionManager.Instance != null && SessionManager.Instance.IsInited);
            hasIntricsSet = false;
            hasPoseOffsetSet = false;
            Debug.Log("-10001-startRGBCamera new rgbCameraDevice Open");
            locCamImageBuffer = new EZVIOInputImage();
            rgbCameraDevice = new NormalRGBCameraDevice();
            rgbCameraDevice.Open();
        }
        private void stopRGBCamera()
        {
            if (rgbCameraDevice != null)
                rgbCameraDevice.Close();
            if (locCamImageBuffer.fullImg != IntPtr.Zero)
            {
                Marshal.FreeHGlobal(locCamImageBuffer.fullImg);
                locCamImageBuffer.fullImg = IntPtr.Zero;
            }
        }

        private IEnumerator startTrackSession()
        {
            yield return new WaitUntil(() => rgbCameraDevice.IsStarted());

            Track3dsession = new EZXRTrack3dSession();

            Debug.Log("-10001-startTrackSession Track3dsession.Create");
            if (Track3dsession != null)
            {
                Track3dsession.Create(Application.persistentDataPath + "/" + algAssetPath);
            }
            InvokeRepeating("UpdateCameraImage", 0.5f, 3.0f);
            Debug.Log("-10001-startTrackSession end");
        }

        private void stopTrackSession()
        {
            Debug.Log("-10001-stopTrackSession");

            hasPoseOffsetSet = false;
            hasIntricsSet = false;

            if (Track3dsession != null)
            {
                Track3dsession.Destroy();
                Track3dsession = null;
            }
            CancelInvoke("UpdateCameraImage");
        }

        private void UpdateCameraImage()
        {
            if (ARFrame.SessionStatus != EZVIOState.EZVIOCameraState_Tracking)
                return;
            if (rgbCameraDevice == null)
            {
                Debug.Log("rgbCameraDevice is null");
                return;
            }
            if (Track3dsession == null)
            {
                Debug.Log("Track3dsession is null");
                return;
            }
            if (!hasPoseOffsetSet)
            {
                NativeTracking.GetOSTParams(ref ost_m);
                Track3dsession.SetPoseFromHeadToLocCam(ost_m.T_RGB_Head);
                hasPoseOffsetSet = true;
            }
            bool res = false;

            //Debug.Log("call getCurrentRGBImage");
            res = rgbCameraDevice.getCurrentImage(ref locCamImageBuffer, locCamIntriarray);
            //Debug.Log("call getCurrentRGBImage " + res);
            if (res)
            {
                if (!hasIntricsSet)
                {
                    //Debug.Log("-10001-UpdateCameraImage Track3dsession.UpdateRGBCameraIntrics");
                    Track3dsession.SetCameraIntrics(locCamIntriarray);
                    hasIntricsSet = true;
                }
                double timestamp_sec = locCamImageBuffer.timestamp;
                Pose imageTsHeadPose = ARFrame.GetHistoricalHeadPose(timestamp_sec);
                Track3dsession.UpdateHeadPose(imageTsHeadPose, timestamp_sec);
                int format = (int)locCamImageBuffer.imgFormat;
                //Debug.Log("-10001-UpdateRgbFrame Track3dsession.UpdateRGBImage:"+ rgbImageBuffer.fullImg+","+timestamp_sec.ToString("f3"));
                //Debug.Log("-10001-UpdateRgbFrame Track3dsession.UpdateRGBImage:" + locCamImageBuffer.imgRes.width + "," + locCamImageBuffer.imgRes.height + ",fotmat=" + locCamImageBuffer.imgFormat);
                Track3dsession.UpdateImage(locCamImageBuffer.fullImg, timestamp_sec, (int)locCamImageBuffer.imgRes.width, (int)locCamImageBuffer.imgRes.height, format);
            }
            Pose pose = Pose.identity;
            bool isRelocSucc = Track3dsession.GettTrackedAnchorPose(ref pose);
            if (isRelocSucc)
            {
                _anchorPose = pose;
                isLocationSuccess = true;
            }
        }
    }
}