private void CalculateTransformation()
{
if (samples_PSMove.Count != numberOfSamplesTaken || samples_Kinect.Count != numberOfSamplesTaken)
{
Debug.LogError("Mismatch in sample list lengths!");
}
Matrix moveMatrix;
Matrix kinectMatrix;
moveMatrix = Matrix.Zeros(samples_PSMove.Count, 4);
kinectMatrix = Matrix.Zeros(samples_Kinect.Count, 3);
for (int i = 1; i <= samples_PSMove.Count; i++)
{
moveMatrix [i, 1] = new Complex(samples_PSMove [i - 1].x);
moveMatrix [i, 2] = new Complex(samples_PSMove [i - 1].y);
moveMatrix [i, 3] = new Complex(samples_PSMove [i - 1].z);
moveMatrix [i, 4] = new Complex(1.0f);
}
for (int i = 1; i <= samples_Kinect.Count; i++)
{
kinectMatrix [i, 1] = new Complex(samples_Kinect [i - 1].x);
kinectMatrix [i, 2] = new Complex(samples_Kinect [i - 1].y);
kinectMatrix [i, 3] = new Complex(samples_Kinect [i - 1].z);
}
//perform a matrix solve Ax = B. We have to get transposes and inverses because moveMatrix isn't square
//the solution is the same with (A^T)Ax = (A^T)B -> x = ((A^T)A)'(A^T)B
Matrix transformMatrixSolution = (moveMatrix.Transpose() * moveMatrix).Inverse() * moveMatrix.Transpose() * kinectMatrix;
// Matrix error = moveMatrix * transformMatrixSolution - kinectMatrix;
transformMatrixSolution = transformMatrixSolution.Transpose();
//Debug.Log(transformMatrixSolution);
//Debug.Log(error);
List <Vector3> orthogonalVectors = MathUtil.Orthonormalize(
MathUtil.ExtractRotationVectors(
MathUtil.MatrixToMatrix4x4(transformMatrixSolution))
);
rotationMatrix = CreateRotationMatrix(orthogonalVectors);
//Debug.Log(rotationMatrix);
transformMatrix = MathUtil.MatrixToMatrix4x4(transformMatrixSolution);
Debug.Log("transformMatrix \n" + transformMatrix);
UpdateFloorNormalAndDistance();
coordinateSystem.SetDeviceToRootTransforms(transformMatrix);
coordinateSystem.SaveTransformDataToXML(xmlFilename, RUISDevice.PS_Move, RUISDevice.Kinect_1);
coordinateSystem.SaveFloorData(xmlFilename, RUISDevice.Kinect_1, kinect1FloorNormal, kinect1DistanceFromFloor);
Quaternion rotationQuaternion = MathUtil.QuaternionFromMatrix(rotationMatrix);
Vector3 translate = new Vector3(transformMatrix[0, 3], transformMatrix[1, 3], transformMatrix[2, 3]);
updateDictionaries(coordinateSystem.RUISCalibrationResultsInVector3,
coordinateSystem.RUISCalibrationResultsInQuaternion,
coordinateSystem.RUISCalibrationResultsIn4x4Matrix,
translate, rotationQuaternion, transformMatrix,
RUISDevice.PS_Move, RUISDevice.Kinect_1);
coordinateSystem.RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_1] = kinect1DistanceFromFloor;
coordinateSystem.RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] = kinect1PitchRotation;
kinect1ModelObject.transform.rotation = kinect1PitchRotation;
kinect1ModelObject.transform.localPosition = new Vector3(0, kinect1DistanceFromFloor, 0);
psEyeModelObject.transform.position = coordinateSystem.ConvertLocation(Vector3.zero, RUISDevice.PS_Move);
psEyeModelObject.transform.rotation = coordinateSystem.ConvertRotation(Quaternion.identity, RUISDevice.PS_Move);
if (this.floorPlane)
{
this.floorPlane.transform.position = new Vector3(0, 0, 0);
}
}
private void CalculateTransformation()
{
if (samples_PSMove.Count != numberOfSamplesTaken || samples_OculusDK2.Count != numberOfSamplesTaken)
{
Debug.LogError("Mismatch in sample list lengths!");
}
Matrix moveMatrix;
Matrix kinectMatrix;
moveMatrix = Matrix.Zeros(samples_PSMove.Count, 4);
kinectMatrix = Matrix.Zeros(samples_OculusDK2.Count, 3);
for (int i = 1; i <= samples_PSMove.Count; i++)
{
moveMatrix [i, 1] = new Complex(samples_PSMove [i - 1].x);
moveMatrix [i, 2] = new Complex(samples_PSMove [i - 1].y);
moveMatrix [i, 3] = new Complex(samples_PSMove [i - 1].z);
moveMatrix [i, 4] = new Complex(1.0f);
}
for (int i = 1; i <= samples_OculusDK2.Count; i++)
{
kinectMatrix [i, 1] = new Complex(samples_OculusDK2 [i - 1].x);
kinectMatrix [i, 2] = new Complex(samples_OculusDK2 [i - 1].y);
kinectMatrix [i, 3] = new Complex(samples_OculusDK2 [i - 1].z);
}
//perform a matrix solve Ax = B. We have to get transposes and inverses because moveMatrix isn't square
//the solution is the same with (A^T)Ax = (A^T)B -> x = ((A^T)A)'(A^T)B
Matrix transformMatrixSolution = (moveMatrix.Transpose() * moveMatrix).Inverse() * moveMatrix.Transpose() * kinectMatrix;
Matrix error = moveMatrix * transformMatrixSolution - kinectMatrix;
transformMatrixSolution = transformMatrixSolution.Transpose();
Debug.Log(transformMatrixSolution);
Debug.Log(error);
List <Vector3> orthogonalVectors = MathUtil.Orthonormalize(
MathUtil.ExtractRotationVectors(
MathUtil.MatrixToMatrix4x4(transformMatrixSolution)
)
);
rotationMatrix = CreateRotationMatrix(orthogonalVectors);
Debug.Log(rotationMatrix);
transformMatrix = MathUtil.MatrixToMatrix4x4(transformMatrixSolution); //CreateTransformMatrix(transformMatrixSolution);
Debug.Log(transformMatrix);
coordinateSystem.SetDeviceToRootTransforms(transformMatrix);
coordinateSystem.SaveTransformDataToXML(xmlFilename, RUISDevice.PS_Move, RUISDevice.Oculus_DK2);
Quaternion rotationQuaternion = MathUtil.QuaternionFromMatrix(rotationMatrix);
Vector3 translate = new Vector3(transformMatrix[0, 3], transformMatrix[1, 3], transformMatrix[2, 3]);
updateDictionaries(coordinateSystem.RUISCalibrationResultsInVector3,
coordinateSystem.RUISCalibrationResultsInQuaternion,
coordinateSystem.RUISCalibrationResultsIn4x4Matrix,
translate, rotationQuaternion, transformMatrix,
RUISDevice.PS_Move, RUISDevice.Oculus_DK2);
psEyeModelObject.transform.position = coordinateSystem.ConvertLocation(Vector3.zero, RUISDevice.PS_Move);
psEyeModelObject.transform.rotation = coordinateSystem.ConvertRotation(Quaternion.identity, RUISDevice.PS_Move);
/*
* string devicePairName = RUISDevice.PS_Move.ToString() + "-" + RUISDevice.Oculus_DK2.ToString();
* coordinateSystem.RUISCalibrationResultsIn4x4Matrix[devicePairName] = transformMatrix;
*
* Quaternion rotationQuaternion = MathUtil.QuaternionFromMatrix(rotationMatrix);
* coordinateSystem.RUISCalibrationResultsInQuaternion[devicePairName] = rotationQuaternion;
*
*/
}
