/// <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); }
private void UpdateRelativeTransform() { convertMatrixRelative = ConvertMatrix.LeftMultiply(convertMatrixWhenSendImagSC, convertMatrixWhenSendImagSLAM); convertMatrixRelative.Inverse(); /* * ConvertMatrix convertMatrixRelativeNew = ConvertMatrix.LeftMultiply(convertMatrixWhenSendImagSC, convertMatrixWhenSendImagSLAM); * convertMatrixRelativeNew.Inverse(); * * // direct assign if not initialized * if (!systemInited) * { * convertMatrixRelative = convertMatrixRelativeNew; * return; * } * * Vector3 changed_position = (convertMatrixRelative.GetPosition() - convertMatrixRelativeNew.GetPosition()); * if(changed_position.magnitude > 1.0) * { * return; * } * * // we choose to use smooth adjustment to offer better user experience * // init variables for coroutine * quaternionRelativeFrom = convertMatrixRelative.GetQuaternion(); * quaternionRelativeTo = convertMatrixRelativeNew.GetQuaternion(); * positionRelativeFrom = convertMatrixRelative.GetPosition(); * positionRelativeTo = convertMatrixRelativeNew.GetPosition(); * * // stop the old coroutine if exist * if (coroutine != null) * { * StopCoroutine(coroutine); * } * * // start the new coroutine * coroutine = WaitAndRotate(0.2f); * StartCoroutine(coroutine); */ }
private void UpdateRelativeTransform() { convertMatrixRelative = ConvertMatrix.LeftMultiply(convertMatrixWhenSendImagSC, convertMatrixWhenSendImagSLAM); convertMatrixRelative.Inverse(); }