JunShi101ShiNei/Assets/XDKUnityPluginCloud/Runtime/Scripts/XDKCloudSession.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using System.Text;
using Unity.Collections.LowLevel.Unsafe;
using UnityEngine;
using UnityEngine.Events;
using Unity.Collections;
using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgcodecsModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.UnityUtils;
using OpenCVForUnity.Calib3dModule;
//using NRKernal;

namespace Seengene.XDKUnityPluginCloud {
    [RequireComponent(typeof(XDKCloudInterface))]
    public class XDKCloudSession : MonoBehaviour {

        #region Properties

        [SerializeField]
        private SvrManager m_SvrManager = null;

        /// <summary>
        /// 重定位间隔时间
        /// </summary>
        private float m_RelocationInterval = 0.1f;
        private float m_RelocationIntervalStart = 0.5f;
        private float m_RelocationIntervalSuccess = 1f;

        /// <summary>
        /// 网络定位SessionID是否初始化？
        /// </summary>
        private bool m_NetSessionIDInitialized = false;
        /// <summary>
        /// 相机内参是否初始化？
        /// 相机内参需要从JIMO机器本地的配置文件中读取
        /// 每台JIMO的相机内参是不相同的
        /// </summary>
        private bool m_CameraCalibrationInitialized = false;
        /// <summary>
        /// 云定位接口
        /// </summary>
        private XDKCloudInterface m_InterfaceInstance;

        /// 上一次空间映射位置、旋转、缩放
        private Vector3 m_PreviousPosition;
        private Quaternion m_PreviousRotation;
        private Vector3 m_PreviousScale;
        /// 空间映射位置、旋转、缩放
        private Vector3 m_SpatialPosition;
        private Quaternion m_SpatialRotation;
        private Vector3 m_SpatialScale;

        /// <summary>
        /// 重定位成功次数
        /// </summary>
        private int m_RelocSuccessCounter = 0;
        /// <summary>
        /// 给算法更新图片次数
        /// </summary>
        private int m_UpdataFrameCounter = 0;

        private Transform m_ARCameraTransform;
        private float m_ElapsedTime;
        private string m_ImageSavePath;
        private Int32 m_LastSeq;
        private float m_Scale;
        private string m_SessionID;
        private RelocationResponse m_LastRelocResponse = null;

        private CameraCalibration m_CameraCalibration = null;
        [SerializeField]
        private CameraCalibrationLoader.EyeType m_EyeType = CameraCalibrationLoader.EyeType.LeftEye;

        #endregion

        #region UnityAction
        /// <summary>
        /// 更新预览图事件
        /// </summary>
        public UnityAction<Texture> UpdatePreviewImageEvent;
        /// <summary>
        /// 空间映射事件
        /// </summary>
        public UnityAction<Vector3, Quaternion, Vector3, int> SpatialMappedEvent;
        /// <summary>
        /// 更新空间特征点事件
        /// </summary>
        public UnityAction<List<Vector3>> UpdateKeyPointsEvent;
        /// <summary>
        /// 首次定位成功事件
        /// </summary>
        public UnityAction OnFirstRelocationSuccessEvent;
        /// <summary>
        /// Debug回调事件
        /// </summary>
        public UnityAction<string, DebugInfoType> OnUpdateDebugInfoEvent;
        #endregion

        #region Unity Methods
        private void Awake() {
            m_InterfaceInstance = GetComponent<XDKCloudInterface>();
        }

        private void Start() {
            m_SpatialPosition = Vector3.zero;
            m_SpatialRotation = Quaternion.identity;
            m_SpatialScale = Vector3.one;
            m_PreviousPosition = Vector3.zero;
            m_PreviousRotation = Quaternion.identity;
            m_PreviousScale = Vector3.one;

            m_UpdataFrameCounter = 0;
            m_RelocSuccessCounter = 0;
            m_ElapsedTime = 0;

            // 清空缓存文文件
            m_ImageSavePath = Path.Combine(Application.persistentDataPath, "seengene");
            if (Directory.Exists(m_ImageSavePath))
                Directory.Delete(m_ImageSavePath, true);

            InintializeCameraCalibration();
            InintializeNetSessionID();
        }

        private void OnEnable() {
            m_InterfaceInstance.OnAuthorized += HandleAuthorizationResponse;
            m_InterfaceInstance.OnRelocated += HandleRelocationResponse;
        }

        private void OnDisable() {
            m_InterfaceInstance.OnAuthorized -= HandleAuthorizationResponse;
            m_InterfaceInstance.OnRelocated -= HandleRelocationResponse;

        }
        private void Update() {

            if (Input.GetKeyUp(KeyCode.Escape)) {
                Resources.UnloadUnusedAssets();
                GC.Collect();
                Application.Quit();
            }

            if (m_NetSessionIDInitialized && m_CameraCalibrationInitialized) {
                m_ElapsedTime += Time.deltaTime;
                if (m_ElapsedTime < m_RelocationInterval) return;
                m_ElapsedTime = 0f;

                GetLatestFrameCameraImageAndPose();
            }

        }

        #endregion


        #region Private Methods
        private void InintializeCameraCalibration() {
            m_CameraCalibration = CameraCalibrationLoader.GetCameraCalibration(m_EyeType);
            if (m_CameraCalibration != null) {
                Debug.LogFormat("初始化相机内参成功：{0}, {1}", m_EyeType, m_CameraCalibration.ToString());
                m_CameraCalibrationInitialized = true;
            } else {
                Debug.LogError("初始化相机内参失败！");
            }
        }

        private void InintializeNetSessionID() {
            m_RelocationInterval = m_RelocationIntervalStart;
            m_InterfaceInstance.Authorize();
        }

        private void HandleAuthorizationResponse(AuthorizationResponse response) {
            if (response.status == (int)MapQueryStatus.MAP_SUCCESS) {
                if (XDKConfigs.IfLogOn) Debug.Log("Authorize Succes!");
                m_SessionID = response.sessionID;
                m_Scale = (float)response.scale;
                m_NetSessionIDInitialized = true;
            } else if (response.status == (int)MapQueryStatus.MAP_FAIL) {
                Debug.LogError("Authorize Failed, Code: " + response.status);
            } else {
                Debug.LogError("Authorize Failed, Undefined Code: " + response.status);
            }
        }

        private void HandleRelocationResponse(RelocationResponse response) {
            switch (response.status) {
                case (int)RelocalizeQueryStatus.SUCCESS:
                    if (XDKConfigs.IfLogOn) Debug.LogFormat("Relocation Succes!  seq: {0}", response.seq);
                    if (response.seq < m_LastSeq) return;
                    m_LastSeq = response.seq;
                    UpdateSpatialRoot(response);
                    break;
                case (int)RelocalizeQueryStatus.FRAME_SUCCESS_BA_FAIL:
                    if (XDKConfigs.IfLogOn)
                        Debug.LogWarningFormat("Relocation Failed, QueryStatus: 1-{0}, seq: {1}", RelocalizeQueryStatus.FRAME_SUCCESS_BA_FAIL, response.seq);
                    break;
                case (int)RelocalizeQueryStatus.MAP_FAIL:
                    if (XDKConfigs.IfLogOn)
                        Debug.LogWarningFormat("Relocation Failed, QueryStatus: 2-{0}, seq: {1}", RelocalizeQueryStatus.MAP_FAIL, response.seq);
                    break;
                case (int)RelocalizeQueryStatus.RELOCALIZE_FAIL:
                    if (XDKConfigs.IfLogOn)
                        Debug.LogWarningFormat("Relocation Failed, QueryStatus: 3-{0}, seq: {1}", RelocalizeQueryStatus.RELOCALIZE_FAIL, response.seq);
                    break;
                case (int)RelocalizeQueryStatus.BA_SESSION_EXPIRED:
                    if (XDKConfigs.IfLogOn)
                        Debug.LogWarningFormat("Relocation Failed, QueryStatus: 4-{0} ,seq: {1}", RelocalizeQueryStatus.BA_SESSION_EXPIRED, response.seq);
                    Debug.LogError("服务器 Session 超时，尝试重新获取Session");
                    m_NetSessionIDInitialized = false;
                    m_SessionID = "";
                    InintializeNetSessionID();
                    break;
                default:
                    break;
            }
        }

        /// <summary>
        /// 更新空间映射根坐标
        /// </summary>
        private void UpdateSpatialRoot(RelocationResponse response) {

            m_RelocSuccessCounter++;

            if (XDKConfigs.IfDebugOn) {
                if (MusicPlayer.Instance != null) {
                    MusicPlayer.Instance.PlayMusic();
                }
            }

            if (m_LastRelocResponse != null) {
                float offset = 0;
                offset = CheckWithProjectPointsAndroid(response);

                if (XDKConfigs.IfLogOn)
                    Debug.LogFormat("offset(反投影偏移量): {0}", offset);

                if (offset <= 10.0f) {
                    m_LastRelocResponse = response;
                    if (OnUpdateDebugInfoEvent != null) {
                        string strDebug = string.Format("重定位成功 {0}! offset = {1},反投影偏移量结果小于10，故放弃本次位姿校正", m_RelocSuccessCounter, offset);
                        OnUpdateDebugInfoEvent.Invoke(strDebug, DebugInfoType.GetSpatialMappingNative);
                    }
                    return;
                }
            }
            m_LastRelocResponse = response;

            List<double> pose = response.transform_ltg;
            if (pose.Count >= 16) {
                Matrix4x4 matrix = new Matrix4x4();
                matrix.m00 = (float)pose[0];
                matrix.m10 = (float)pose[1];
                matrix.m20 = (float)pose[2];
                matrix.m30 = (float)pose[3];
                matrix.m10 = (float)pose[4];
                matrix.m11 = (float)pose[5];
                matrix.m21 = (float)pose[6];
                matrix.m31 = (float)pose[7];
                matrix.m02 = (float)pose[8];
                matrix.m12 = (float)pose[9];
                matrix.m22 = (float)pose[10];
                matrix.m32 = (float)pose[11];
                matrix.m03 = (float)pose[12];
                matrix.m13 = (float)pose[13];
                matrix.m23 = (float)pose[14];
                matrix.m33 = (float)pose[15];

                matrix = matrix.transpose;

                matrix.m01 = -matrix.m01;
                matrix.m02 = -matrix.m02;
                matrix.m10 = -matrix.m10;
                matrix.m13 = -matrix.m13;
                matrix.m20 = -matrix.m20;
                matrix.m23 = -matrix.m23;

                m_SpatialPosition = matrix.GetPosition();
                m_SpatialPosition.z = -m_SpatialPosition.z;
                m_SpatialRotation = matrix.GetRotation();
                m_SpatialRotation.z = -m_SpatialRotation.z;
                m_SpatialRotation.w = -m_SpatialRotation.w;
                m_SpatialScale = new Vector3(m_Scale, m_Scale, m_Scale);
            }

            if (m_PreviousPosition != m_SpatialPosition || m_PreviousRotation != m_SpatialRotation || m_PreviousScale != m_SpatialScale) {
                m_PreviousPosition = m_SpatialPosition;
                m_PreviousRotation = m_SpatialRotation;
                m_PreviousScale = m_SpatialScale;

                SpatialMappedEvent?.Invoke(m_SpatialPosition, m_SpatialRotation, m_SpatialScale, m_RelocSuccessCounter);

                if (m_RelocSuccessCounter == 1) {
                    m_RelocationInterval = m_RelocationIntervalSuccess;
                    if (OnFirstRelocationSuccessEvent != null) {
                        OnFirstRelocationSuccessEvent.Invoke();
                    }
                }

                if (XDKConfigs.IfDebugOn) {
                    if (response.point3d_vec != null && UpdateKeyPointsEvent != null) {
                        UpdateKeyPointsEvent.Invoke(response.point3d_vec);
                    }
                    if (MusicPlayer.Instance != null) {
                        MusicPlayer.Instance.PlayMusic();
                    }

                    string strDebug = string.Format("重定位成功 {0} : {1},{2},{3},{4}", m_RelocSuccessCounter, m_SpatialPosition.ToString("f2"),
                        m_SpatialRotation.ToString("f2"), m_SpatialRotation.ToString("f2"), m_SpatialScale.ToString("f2"));
                    if (XDKConfigs.IfLogOn) {
                        Debug.Log(strDebug);
                    }
                    if (OnUpdateDebugInfoEvent != null)
                        OnUpdateDebugInfoEvent.Invoke(strDebug, DebugInfoType.GetSpatialMappingNative);
                }
            }
        }

        private float CheckWithProjectPointsAndroid(RelocationResponse response) {
            // MatOfPoint3f objectPoints
            MatOfPoint3f objectPoints = new MatOfPoint3f();
            List<Point3> listPoint3 = new List<Point3>();
            for (int i = 0; i < response.point3d_vec.Count; i++) {
                Vector3 v3 = response.point3d_vec[i];
                Point3 p = new Point3(v3.x, v3.y, v3.z);
                listPoint3.Add(p);
            }
            objectPoints.fromList(listPoint3);

            //Debug.LogFormat("objectPoints; {0}", objectPoints.dump());

            // Mat rvec(Rotation vector)
            // Mat tvec(Translation vector)
            Mat p1T = new Mat(4, 4, CvType.CV_32FC1);
            Matrix4x4 matrixCam = response.cameraPos;
            float[] arrCamTrans =
                new float[] { matrixCam.m00, matrixCam.m10, matrixCam.m20, matrixCam.m30,
                              matrixCam.m01, matrixCam.m11, matrixCam.m21, matrixCam.m31,
                              matrixCam.m02, matrixCam.m12, matrixCam.m22, matrixCam.m32,
                              matrixCam.m03, matrixCam.m13, matrixCam.m23, matrixCam.m33};

            //float[] arrCamTrans =
            //    new float[] { matrixCam.m00, matrixCam.m01, matrixCam.m02, matrixCam.m03,
            //                  matrixCam.m10, matrixCam.m11, matrixCam.m12, matrixCam.m13,
            //                  matrixCam.m20, matrixCam.m21, matrixCam.m22, matrixCam.m23,
            //                  matrixCam.m30, matrixCam.m31, matrixCam.m32, matrixCam.m33};

            p1T.put(0, 0, arrCamTrans);

            //Debug.LogFormat("p1T(Before): {0}", p1T.dump());

            p1T.put(0, 1, -p1T.get(0, 1)[0]);
            p1T.put(0, 2, -p1T.get(0, 2)[0]);
            p1T.put(1, 0, -p1T.get(1, 0)[0]);
            p1T.put(1, 3, -p1T.get(1, 3)[0]);
            p1T.put(2, 0, -p1T.get(2, 0)[0]);
            p1T.put(2, 3, -p1T.get(2, 3)[0]);

            //Debug.LogFormat("p1T(After): {0}", p1T.dump());
            //Debug.LogFormat("p1T.row(3): {0}", p1T.row(3).dump());

            List<double> pose = m_LastRelocResponse.transform_ltg;
            Matrix4x4 matrixMap = new Matrix4x4();
            matrixMap.m00 = (float)pose[0];
            matrixMap.m10 = (float)pose[1];
            matrixMap.m20 = (float)pose[2];
            matrixMap.m30 = (float)pose[3];
            matrixMap.m01 = (float)pose[4];
            matrixMap.m11 = (float)pose[5];
            matrixMap.m21 = (float)pose[6];
            matrixMap.m31 = (float)pose[7];
            matrixMap.m02 = (float)pose[8];
            matrixMap.m12 = (float)pose[9];
            matrixMap.m22 = (float)pose[10];
            matrixMap.m32 = (float)pose[11];
            matrixMap.m03 = (float)pose[12];
            matrixMap.m13 = (float)pose[13];
            matrixMap.m23 = (float)pose[14];
            matrixMap.m33 = (float)pose[15];

            float[] arrMapTrans =
                new float[] { matrixMap.m00, matrixMap.m10, matrixMap.m20, matrixMap.m30,
                              matrixMap.m01, matrixMap.m11, matrixMap.m21, matrixMap.m31,
                              matrixMap.m02, matrixMap.m12, matrixMap.m22, matrixMap.m32,
                              matrixMap.m03, matrixMap.m13, matrixMap.m23, matrixMap.m33};

            //float[] arrMapTrans =
            //    new float[] { matrixMap.m00, matrixMap.m01, matrixMap.m02, matrixMap.m03,
            //                  matrixMap.m10, matrixMap.m11, matrixMap.m12, matrixMap.m13,
            //                  matrixMap.m20, matrixMap.m21, matrixMap.m22, matrixMap.m23,
            //                  matrixMap.m30, matrixMap.m31, matrixMap.m32, matrixMap.m33};

            Mat p2T = new Mat(4, 4, CvType.CV_32FC1);
            p2T.put(0, 0, arrMapTrans);

            //Debug.LogFormat("p2T: {0}", p2T.dump());
            //Debug.LogFormat("p2T.row(3): {0}", p2T.row(3).dump());

            Mat t12T = p1T.inv() * p2T;
            //Debug.LogFormat("t12T; {0}", t12T.dump());
            Mat cvMat = Mat.zeros(3, 3, t12T.type());
            Mat t = Mat.zeros(3, 1, t12T.type());
            t12T.rowRange(0, 3).colRange(0, 3).copyTo(cvMat);
            t12T.rowRange(0, 3).col(3).copyTo(t);
            Mat r = new Mat();
            Calib3d.Rodrigues(cvMat, r);
            //Debug.LogFormat("r: {0}", r.dump());
            //Debug.LogFormat("t: {0}", t.dump());

            // Mat cameraMatrix (Intrisic matrix)
            Mat intrisicMat = new Mat(3, 3, CvType.CV_64FC1);
            float focalLengthX = response.cameraCalibration.focal_length.x;
            float focalLengthY = response.cameraCalibration.focal_length.y;
            float principalPointX = response.cameraCalibration.principal_point.x;
            float principalPointY = response.cameraCalibration.principal_point.y;

            intrisicMat.put(0, 0,
                focalLengthX, 0, principalPointX,
                0, focalLengthY, principalPointY,
                0, 0, 1);

            //Debug.LogFormat("intrisicMat: {0}", intrisicMat.dump());

            // MatOfDouble distCoeffs
            MatOfDouble distCoeffs = new MatOfDouble(0, 0, 0, 0);
            //Debug.LogFormat("distCoeffs: {0}", distCoeffs.dump());

            // MatOfPoint2f imagePoints
            MatOfPoint2f tempVecPoints2D = new MatOfPoint2f();
            //Debug.LogFormat("tempVecPoints2D(before): {0}", tempVecPoints2D.dump());

            Calib3d.projectPoints(objectPoints, r, t, intrisicMat, distCoeffs, tempVecPoints2D);
            //Debug.LogFormat("tempVecPoints2D(after): {0}", tempVecPoints2D.dump());

            List<float> listDistance = new List<float>();
            for (int i = 0; i < tempVecPoints2D.toArray().Length; i++) {
                Point p = tempVecPoints2D.toArray()[i];
                Vector2 v2 = response.point2d_vec[i];
                float dis = Mathf.Pow(((float)p.x - v2.x) * ((float)p.x - v2.x) + ((float)p.y - v2.y) * ((float)p.y - v2.y), 0.5f);
                listDistance.Add(dis);
            }
            listDistance.Sort();
            //Debug.LogFormat("listDistance: {0}", Tools.ListFloatToString(listDistance));

            float total = 0;
            int num = 0;
            float last = 0;
            for (int i = 0; i < listDistance.Count; i++) {
                float f = listDistance[i];
                if (num > listDistance.Count / 2) {
                    if (f > last * 3 && f > 10) {
                        break;
                    }
                }
                total = total + f;
                last = f;
                num++;
            }
            return total / num;
        }


        private void GetLatestFrameCameraImageAndPose() {

            if (!SvrManager.Instance.Initialized) return;
            if (!XDKConfigs.IfRelocationOn) return;

            int seq = m_UpdataFrameCounter++;

            int imageWidth = 640;
            int imageHeight = 400;
            bool outBUdate = true;
            uint outCurrFrameIndex = 0;
            ulong outFrameExposureNano = 0;
            byte[] outFrameData = new byte[imageWidth * imageHeight];
            float[] outTRDataArray = new float[7];
            TextureFormat textureFormat = TextureFormat.R8;

            // SvrPluginAndroid.SvrGetLatestQVRCameraFrameData(ref outBUdate, ref outCurrFrameIndex, ref outFrameExposureNano, outFrameData, outTRDataArray);
            SvrPluginAndroid.SvrGetLatestQVRCameraFrameDataNoTransform(ref outBUdate, ref outCurrFrameIndex, ref outFrameExposureNano, outFrameData, outTRDataArray);

            if (outBUdate) {

                /// 获取相机位姿 从接口
                Vector3 cameraPosition = new Vector3(outTRDataArray[4], outTRDataArray[5], outTRDataArray[6]);
                Quaternion cameraRotation = new Quaternion(outTRDataArray[0], outTRDataArray[1], outTRDataArray[2], outTRDataArray[3]);
                Debug.LogFormat("接口数据：position:{0},rotation:{1}", cameraPosition, cameraRotation);
                Vector3 posIn = cameraPosition;
                posIn.z = -posIn.z;
                Quaternion rotIn = cameraRotation;
                rotIn.z = -rotIn.z;
                rotIn.w = -rotIn.w;


                /// 获取head位姿，直接从场景中的head物体
                Transform head = m_SvrManager.head;
                Debug.LogFormat("Head：position:{0},rotation:{1}", head.position, head.rotation);
                //Vector3 posIn_Head = head.position;
                //posIn_Head.z = -posIn_Head.z;
                //Quaternion rotIn_Head = head.rotation;
                //rotIn_Head.z = -rotIn_Head.z;
                //rotIn_Head.w = -rotIn_Head.w;


                Matrix4x4 matFinal = Matrix4x4.TRS(posIn, rotIn, Vector3.one);
                matFinal = matFinal.transpose;

                // compress grayscale image data into JPG format.
                byte[] jpegBytes;
                jpegBytes = XDKTools.GetGrayTextureBytes(outFrameData, textureFormat, imageHeight, imageWidth);

                if (jpegBytes == null) {
                    Debug.LogErrorFormat("第{0}张图片转单通道灰度图JPG格式时出错！", seq);
                    return;
                }

                if (XDKConfigs.IfDebugOn) {
                    string strDebug = string.Format("更新图片 {0}:", seq);
                    if (OnUpdateDebugInfoEvent != null)
                        OnUpdateDebugInfoEvent.Invoke(strDebug, DebugInfoType.UpdateFrameNative);

                    if (XDKConfigs.IfSaveImages) {
                        Texture2D texture = new Texture2D(imageWidth, imageHeight, textureFormat, false);
                        texture.LoadImage(jpegBytes);
                        texture.Apply();
                        UpdatePreviewImageEvent?.Invoke(texture);
                        SaveCameraImageToLocal(seq, jpegBytes);
                        SaveCameraPoseDataToLocal(seq, matFinal, m_CameraCalibration);
                        SaveCameraPoseOriginDataToLocal(seq, cameraPosition, cameraRotation, m_CameraCalibration);
                        Debug.LogFormat("保存第{0}张图片成功！", seq);
                    }
                }

                m_InterfaceInstance.Relocate(m_SessionID, seq, matFinal, m_CameraCalibration, jpegBytes);
            }
        }

        private void SaveCameraImageToLocal(int seq, byte[] imageBuffer) {
            if (!Directory.Exists(m_ImageSavePath)) Directory.CreateDirectory(m_ImageSavePath);
            File.WriteAllBytes(Path.Combine(m_ImageSavePath, seq + ".jpg"), imageBuffer);
        }

        private void SaveCameraPoseDataToLocal(int seq, Matrix4x4 cameraPose, CameraCalibration cameraCalibration) {
            if (!Directory.Exists(m_ImageSavePath)) Directory.CreateDirectory(m_ImageSavePath);

            // 计算相机内参
            float intrinsics_00 = cameraCalibration.focal_length.x;
            float intrinsics_01 = 0;
            float intrinsics_02 = cameraCalibration.principal_point.x;
            float intrinsics_10 = 0;
            float intrinsics_11 = cameraCalibration.focal_length.y;
            float intrinsics_12 = cameraCalibration.principal_point.y;
            float intrinsics_20 = 0;
            float intrinsics_21 = 0;
            float intrinsics_22 = 1;

            string poseData = string.Format(
                                "{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13} {14} {15} {16} {17} {18} {19} {20} {21} {22} {23} {24}",
                                cameraPose.m00, cameraPose.m01, cameraPose.m02, cameraPose.m03,
                                cameraPose.m10, cameraPose.m11, cameraPose.m12, cameraPose.m13,
                                cameraPose.m20, cameraPose.m21, cameraPose.m22, cameraPose.m32,
                                cameraPose.m30, cameraPose.m31, cameraPose.m32, cameraPose.m33,
                                intrinsics_00, intrinsics_01, intrinsics_02,
                                 intrinsics_10, intrinsics_11, intrinsics_12,
                                 intrinsics_20, intrinsics_21, intrinsics_22);

            File.WriteAllText(Path.Combine(m_ImageSavePath, seq + ".txt"), poseData);
        }

        private void SaveCameraPoseOriginDataToLocal(int seq, Vector3 cameraPosition, Quaternion cameraRotation, CameraCalibration cameraCalibration) {
            if (!Directory.Exists(m_ImageSavePath)) Directory.CreateDirectory(m_ImageSavePath);

            string poseDataOrigin = string.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13} {14} {15}",
                                cameraPosition.x, cameraPosition.y, cameraPosition.z, cameraRotation.x, cameraRotation.y, cameraRotation.z, cameraRotation.w,
                               cameraCalibration.focal_length.x, 0, cameraCalibration.principal_point.x,
                                0, cameraCalibration.focal_length.y, cameraCalibration.principal_point.y,
                                0, 0, 1);

            File.WriteAllText(Path.Combine(m_ImageSavePath, seq + "-origin" + ".txt"), poseDataOrigin);
        }

        #endregion

        #region Public Methods

        /// <summary>
        /// 手动模拟首次定位成功
        /// </summary>
        public void SimulateFirstRelocatSuccess() {
            if (m_RelocSuccessCounter >= 1) return;
            m_RelocSuccessCounter = 1;
            if (OnFirstRelocationSuccessEvent != null) {
                OnFirstRelocationSuccessEvent.Invoke();
            }
        }

        #endregion
    }
}