public override RUISCalibrationPhase ShowResultsPhase(float deltaTime)
{
if (!calibrationFinnished)
{
this.guiTextLowerLocal = "Calibration finished!\n\nDistance from floor: " + kinect2DistanceFromFloor + "\n\nFloor normal: " + kinect2FloorNormal.ToString();
coordinateSystem.SaveFloorData(xmlFilename, RUISDevice.Kinect_2, kinect2FloorNormal, kinect2DistanceFromFloor);
calibrationFinnished = true;
}
return(RUISCalibrationPhase.ShowResults);
}
public override RUISCalibrationPhase ShowResultsPhase(float deltaTime)
{
if (!calibrationFinnished)
{
if (kinectError)
{
this.guiTextLowerLocal = "Error: Could not read Kinect floor data!";
}
else
{
this.guiTextLowerLocal = "Calibration finished!\n\nDistance from floor: " + kinect1DistanceFromFloor
+ "\n\nFloor normal: " + normalVector.ToString();
coordinateSystem.SaveFloorData(xmlFilename, RUISDevice.Kinect_1, normalVector, kinect1DistanceFromFloor);
}
calibrationFinnished = true;
}
return(RUISCalibrationPhase.ShowResults);
}
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);
}
}
