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