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