/// <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);
}
convertMatrixWhenSendImagSC = new ConvertMatrix(quaternionWhenSendImageSC, positionWhenSendImageSC);
convertMatrixWhenSendImagSLAM = new ConvertMatrix(cameraPoseInput, true, colmapScale);
//convertMatrixWhenSendImagSLAM.Inverse();
UpdateRelativeTransform();
systemInited = true;
ifWaitingForResponse = false;
return(true);
}
private IEnumerator WaitAndRotate(float waitTime)
{
for (int i = 0; i < 30; i++)
{
float t = i * 0.1f;
Quaternion tmpQ = Quaternion.Lerp(quaternionRelativeFrom, quaternionRelativeTo, t);
Vector3 tmpP = Vector3.Lerp(positionRelativeFrom, positionRelativeTo, t);
convertMatrixRelative = new ConvertMatrix(tmpQ, tmpP);
yield return(new WaitForSeconds(waitTime));
}
}
/*
* // if choose to use average method. TODO: find the proper way to average
* convertMatrixRelative = ConvertMatrix.AverageWithFactor(convertMatrixRelative, totalNumber, newconvertMatrixRelative, 1);
*/
static public ConvertMatrix AverageWithFactor(ConvertMatrix convertMatrix1, float factor1, ConvertMatrix convertMatrix2, float factor2)
{
float factorSum = factor1 + factor2;
//Vector3 newAngles = (factor1 * convertMatrix1.quaternion.eulerAngles + factor2 * convertMatrix2.quaternion.eulerAngles)/ factorSum;
//Quaternion quaternionN = Quaternion.Euler(newAngles);
Quaternion quaternionN = convertMatrix1.quaternion;
Vector3 positionN = (convertMatrix1.position * factor1 + convertMatrix2.position * factor2) / factorSum;
return(new ConvertMatrix(quaternionN, positionN));
}
static public ConvertMatrix LeftMultiply(ConvertMatrix convertMatrix1, ConvertMatrix convertMatrix2)
{
// calculate with quaternion and position R1*(R2*p+t2)+t1 -> R1*R2*p + R1*t2 + t1
Quaternion quaternionN = convertMatrix1.quaternion * convertMatrix2.quaternion;
Vector3 positionN = convertMatrix1.quaternion * convertMatrix2.position + convertMatrix1.position;
return(new ConvertMatrix(quaternionN, positionN));
/*
* // calculate by general matrix multiply
* float[,] mat = MultiplyMatrix(convertMatrix1.cameraPose, convertMatrix2.cameraPose);
* return new ConvertMatrix(TransformMatrixToArray(mat), false);
*/
}
/// <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);
}
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();
}
