/// <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();
}