/// <summary> /// need to be called after received the message to the tell the system pose of world coordinate /// so that we can calcualte the difference between ShadowCreator coordinate system and world coordinate system /// </summary> /// <param name="cameraPoseInput"></param> /// <returns></returns> public bool ReceivePoseFromServer(float[] cameraPoseInput) { if (!ifWaitingForResponse) { return(false); } if (cameraPoseInput[15] == 0) { ifWaitingForResponse = false; return(false); } Vector3 tmp = new Vector3(0, 0, 0); if (!ifFirst) { tmp = convertMatrixWhenSendImagSLAM.GetPosition(); } convertMatrixWhenSendImagSC = new ConvertMatrix(quaternionWhenSendImageSC, positionWhenSendImageSC); convertMatrixWhenSendImagSLAM = new ConvertMatrix(cameraPoseInput, true, colmapScale); if (!ifFirst) { lastLocalPose = positionWhenSendImageSC; Vector3 dis = tmp - convertMatrixWhenSendImagSLAM.GetPosition(); DistanceGlobal += dis.magnitude; DistanceLocal += DistanceLocalTmp; UpdateEstimatorScale(); } else { lastLocalPose = positionWhenSendImageSC; ifFirst = false; } //convertMatrixWhenSendImagSLAM.Inverse(); UpdateRelativeTransform(); systemInited = true; ifWaitingForResponse = false; return(true); }
/// <summary> /// poseOutput.Inverse = poseShadowCreator_t.Inverse * poseShadowCreator_old * poseSLAM_old.Inverse(); /// </summary> /// <param name="quaternion"></param> /// <param name="position"></param> /// <param name="outputQuaternion"></param> /// <param name="outputPosition"></param> public bool CalculateCameraPoseInWorldCoordinate(ref Quaternion outputQuaternion, ref Vector3 outputPosition) { //modifyPose = ""; if (!systemInited) { //Debug.Log("System Not initialized."); return(false); } Quaternion quaternion = slamCamera.transform.localRotation; Vector3 position = slamCamera.transform.localPosition; // pose of the object with respect to the local coordinate system (shadow creator local coordinate system) convertMatrixNow = new ConvertMatrix(quaternion, position); ConvertMatrix convertMatrixOutput = ConvertMatrix.LeftMultiply(convertMatrixRelative, convertMatrixNow); outputQuaternion = convertMatrixOutput.GetQuaternion(); outputPosition = convertMatrixOutput.GetPosition(); return(true); }