GHZSDKXR/Runtime/Recording/Scripts/PhonePoseTrackerRGB.cs

using EZXR.Glass;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using EZXR.Glass.SixDof;
using static EZXR.Glass.SixDof.HMDPoseTracker;
using EZXR.Glass.Core;
using EZXR.Glass.Device;

namespace EZXR.Glass.Recording
{
    public class PhonePoseTrackerRGB : MonoBehaviour
    {
        private static PhonePoseTrackerRGB _instance;
        public static PhonePoseTrackerRGB Instance
        {
            get
            {
                return _instance;
            }
        }

        private Transform m_Transform;

        private static int picwith;
        private static int picheight;

        Matrix4x4 offset_SpatialComputing = Matrix4x4.identity;
        Vector3 tempPosition;
        Quaternion tempRotation;
        Matrix4x4 tempMatrix;

        //https://gitfub.space/Pownie/arskrald/blob/716b28b3e55d0d2e253dc346ccc79b23a08bcd1c/AR-3/Assets/OpenCVForUnity/org/opencv/unity/ARUtils.cs
        static Matrix4x4 CalculateProjectionMatrixFromCameraMatrixValues(float fx, float fy, float cx, float cy, float width, float height, float near, float far)
        {
            Matrix4x4 projectionMatrix = new Matrix4x4();
            projectionMatrix.m00 = 2.0f * fx / width;
            projectionMatrix.m02 = 1.0f - 2.0f * cx / width;
            projectionMatrix.m11 = 2.0f * fy / height;
            projectionMatrix.m12 = -1.0f + 2.0f * cy / height;
            projectionMatrix.m22 = -(far + near) / (far - near);
            projectionMatrix.m23 = -2.0f * far * near / (far - near);
            projectionMatrix.m32 = -1.0f;

            return projectionMatrix;
        }


        private void Awake()
        {
            _instance = this;
            m_Transform = transform;
        }

        // Start is called before the first frame update
        void Start()
        {
            if (!Application.isEditor)
            {
                //@buqing init param when start
                StartCoroutine(InitParam());
            }
        }

        // Update is called once per frame
        void Update()
        {
            UpdatePoseByTrackingType();
        }

        private IEnumerator InitParam()
        {
            yield return new WaitUntil(() => SessionManager.Instance != null && SessionManager.Instance.IsInited);
            Camera camera = this.gameObject.GetComponent<Camera>();
            NormalRGBCameraDevice rgbCameraDevice = new NormalRGBCameraDevice();
            float[] intrinsic = new float[8];
            rgbCameraDevice.getRGBCameraIntrics(intrinsic);
            float fx = intrinsic[0];
            float fy = intrinsic[1];
            float cx = intrinsic[2];
            float cy = intrinsic[3];

            Debug.Log("=============Unity Log===============   PhonePoseTrackerRGB -- InitParam   rgb intrinsic " + fx + " " + fy + " " + cx + " " + cy);
            
            int[] sizeRgbCamera = rgbCameraDevice.getCameraSize();
            picwith = sizeRgbCamera[0];
            picheight = sizeRgbCamera[1];
            camera.projectionMatrix = CalculateProjectionMatrixFromCameraMatrixValues(fx, fy, cx, cy, picwith, picheight, camera.nearClipPlane, camera.farClipPlane);

        }

        private void UpdatePoseByTrackingType()
        {
            // ARFrame.OnFixedUpdate();
            if (ARFrame.SessionStatus != EZVIOState.EZVIOCameraState_Tracking) return;
            //Pose phonePose = ARFrame.GetRGBCameraPose();
            Pose phonePose = ARFrame.GetLatestPose_RGBCamera();
            DegreeOfFreedom degreeOfFreedom = HMDPoseTracker.Instance.degreeOfFreedom;

            if (HMDPoseTracker.Instance.UseLocalPose)
            {
                if (degreeOfFreedom != DegreeOfFreedom.ZeroDof)
                {
                    if (degreeOfFreedom == DegreeOfFreedom.SixDof)
                    {
                        m_Transform.localPosition = phonePose.position;
                    }
                    m_Transform.localRotation = phonePose.rotation;
                }
            }
            else
            {
                if (degreeOfFreedom != DegreeOfFreedom.ZeroDof)
                {
                    if (degreeOfFreedom == DegreeOfFreedom.SixDof)
                    {
                        m_Transform.position = phonePose.position;
                    }
                    m_Transform.rotation = phonePose.rotation;
                }
            }

//            if (XRMan.Instance != null)
//            {
//#if SpatialComputing
//                if (EZXRSpatialComputingManager.Instance != null)
//                {
//                    tempMatrix = offset_SpatialComputing * Matrix4x4.TRS(m_Transform.position, m_Transform.rotation, Vector3.one);
//                    tempPosition = tempMatrix.GetColumn(3);
//                    tempRotation = tempMatrix.rotation;
//                }
//                m_Transform.position = tempPosition;
//                m_Transform.rotation = tempRotation;
//#endif
//            }
        }

    }

}