void Update() { if (RUISOVRManager.ovrHmd != null) { Vector3 tempSample = Vector3.zero; Ovr.Posef headpose = RUISOVRManager.ovrHmd.GetTrackingState().HeadPose.ThePose; float px = headpose.Position.x; float py = headpose.Position.y; float pz = -headpose.Position.z; // This needs to be negated TODO: might change with future OVR version tempSample = new Vector3(px, py, pz); tempSample = coordinateSystem.ConvertRawOculusDK2Location(tempSample); Vector3 convertedLocation = coordinateSystem.ConvertLocation(tempSample, RUISDevice.Oculus_DK2); this.transform.localPosition = convertedLocation; if (OVRManager.capiHmd != null) { try { this.transform.localRotation = OVRManager.capiHmd.GetTrackingState().HeadPose.ThePose.Orientation.ToQuaternion(); } catch (System.Exception e) { Debug.LogError(e.Message); } } } }
public override void PlaceSensorModels() { kinect1ModelObject.transform.rotation = kinect1PitchRotation; kinect1ModelObject.transform.localPosition = new Vector3(0, kinect1DistanceFromFloor, 0); // kinect2ModelObject.transform.position = transformMatrix.MultiplyPoint3x4(kinect2ModelObject.transform.position); // kinect2ModelObject.transform.rotation = QuaternionFromMatrix(rotationMatrix) * kinect1PitchRotation; // kinect2ModelObject.transform.localPosition += new Vector3(0, kinect1DistanceFromFloor, 0); kinect2ModelObject.transform.position = coordinateSystem.ConvertLocation(Vector3.zero, RUISDevice.Kinect_2); kinect2ModelObject.transform.rotation = coordinateSystem.ConvertRotation(Quaternion.identity, RUISDevice.Kinect_2); if (this.floorPlane) { this.floorPlane.transform.position = new Vector3(0, 0, 0); } }
void Update() { // if(RUISOVRManager.ovrHmd != null) //06to08 if (RUISDisplayManager.IsHmdPresent()) { Vector3 tempSample = Vector3.zero; // Ovr.Posef headpose = RUISOVRManager.ovrHmd.GetTrackingState().HeadPose.ThePose; // float px = headpose.Position.x; // float py = headpose.Position.y; // float pz = -headpose.Position.z; // This needs to be negated TODO: might change with future OVR version // // tempSample = new Vector3(px, py, pz); // // tempSample = coordinateSystem.ConvertRawOculusDK2Location(tempSample); // HACK TODO if this doesn't work for major HMDs, add wrapper (also for rotation) // HACK TODO tempSample components might need negation or other hackery tempSample = UnityEngine.XR.InputTracking.GetLocalPosition(UnityEngine.XR.XRNode.Head); //06to08 Vector3 convertedLocation = coordinateSystem.ConvertLocation(tempSample, RUISDevice.OpenVR); this.transform.localPosition = convertedLocation; this.transform.localRotation = UnityEngine.XR.InputTracking.GetLocalRotation(UnityEngine.XR.XRNode.Head); // HACK TODO check that this works // if(OVRManager.capiHmd != null) // { // try // { // this.transform.localRotation = OVRManager.capiHmd.GetTrackingState().HeadPose.ThePose.Orientation.ToQuaternion(); // } // catch(System.Exception e) // { // Debug.LogError(e.Message); // } // } } }
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); } }
void Update() { if (inputManager.enableKinect) { for (int i = 0; i < playerManager.m_MaxNumberOfPlayers; i++) { skeletons [kinect1SensorID, i].isTracking = playerManager.GetPlayer(i).Tracking; if (!skeletons [kinect1SensorID, i].isTracking) { continue; } UpdateKinectRootData(i); UpdateKinectJointData(OpenNI.SkeletonJoint.Head, i, ref skeletons [kinect1SensorID, i].head); UpdateKinectJointData(OpenNI.SkeletonJoint.Torso, i, ref skeletons [kinect1SensorID, i].torso); UpdateKinectJointData(OpenNI.SkeletonJoint.LeftShoulder, i, ref skeletons [kinect1SensorID, i].leftShoulder); UpdateKinectJointData(OpenNI.SkeletonJoint.LeftElbow, i, ref skeletons [kinect1SensorID, i].leftElbow); UpdateKinectJointData(OpenNI.SkeletonJoint.LeftHand, i, ref skeletons [kinect1SensorID, i].leftHand); UpdateKinectJointData(OpenNI.SkeletonJoint.RightShoulder, i, ref skeletons [kinect1SensorID, i].rightShoulder); UpdateKinectJointData(OpenNI.SkeletonJoint.RightElbow, i, ref skeletons [kinect1SensorID, i].rightElbow); UpdateKinectJointData(OpenNI.SkeletonJoint.RightHand, i, ref skeletons [kinect1SensorID, i].rightHand); UpdateKinectJointData(OpenNI.SkeletonJoint.LeftHip, i, ref skeletons [kinect1SensorID, i].leftHip); UpdateKinectJointData(OpenNI.SkeletonJoint.LeftKnee, i, ref skeletons [kinect1SensorID, i].leftKnee); UpdateKinectJointData(OpenNI.SkeletonJoint.LeftFoot, i, ref skeletons [kinect1SensorID, i].leftFoot); UpdateKinectJointData(OpenNI.SkeletonJoint.RightHip, i, ref skeletons [kinect1SensorID, i].rightHip); UpdateKinectJointData(OpenNI.SkeletonJoint.RightKnee, i, ref skeletons [kinect1SensorID, i].rightKnee); UpdateKinectJointData(OpenNI.SkeletonJoint.RightFoot, i, ref skeletons [kinect1SensorID, i].rightFoot); } } if (inputManager.enableKinect2) { bool isNewFrame = false; Kinect.Body[] data = RUISKinect2Data.getData(out isNewFrame); isNewKinect2Frame = isNewFrame; if (data != null && isNewFrame) { Vector3 relativePos; int playerID = 0; bool newBody = true; int i = 0; // Refresh skeleton tracking status for (int y = 0; y < skeletons.GetLength(1); y++) { skeletons [kinect2SensorID, y].isTracking = false; } foreach (var body in data) { if (body.IsTracked) { for (int y = 0; y < skeletons.GetLength(1); y++) { if (skeletons [kinect2SensorID, y].trackingId == body.TrackingId) { skeletons [kinect2SensorID, y].isTracking = true; } } } } foreach (var body in data) { if (i >= skeletons.GetLength(1)) { break; } if (body == null) { continue; } newBody = true; playerID = 0; // Check if trackingID has been assigned to certaint index before and use that index if (trackingIDtoIndex.ContainsKey(body.TrackingId) && body.IsTracked) { playerID = trackingIDtoIndex[body.TrackingId]; newBody = false; } if (body.IsTracked) { if (newBody) { // Find the first unused slot in skeletons array for (int y = 0; y < skeletons.GetLength(1); y++) { if (!skeletons [kinect2SensorID, y].isTracking) { playerID = y; break; } } } trackingIDtoIndex[body.TrackingId] = playerID; skeletons [kinect2SensorID, playerID].trackingId = body.TrackingId; // HACK TO make things faster, remove later // if(playerID > 0) // return; // HACK: Kinect 2 can't track closer than 0.5 meters! if (body.Joints[Kinect.JointType.SpineMid].Position.Z < 0.5f) { skeletons [kinect2SensorID, playerID].isTracking = false; } UpdateKinect2RootData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineMid], body.JointOrientations[Kinect.JointType.SpineMid]), playerID); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.Head], body.JointOrientations[Kinect.JointType.Head]), playerID, ref skeletons [kinect2SensorID, playerID].head); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.Neck], body.JointOrientations[Kinect.JointType.Neck]), playerID, ref skeletons [kinect2SensorID, playerID].neck); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineMid], body.JointOrientations[Kinect.JointType.SpineMid]), playerID, ref skeletons [kinect2SensorID, playerID].torso); // Kinect 2 SpineMid position adjusted to correspond to Kinect 1's torso position (so that hip-torso segment doesn't stretch): // First get SpineBase position tempVector = coordinateSystem.ConvertLocation(coordinateSystem.ConvertRawKinect2Location( GetKinect2JointData(body.Joints[Kinect.JointType.SpineBase], body.JointOrientations[Kinect.JointType.SpineMid]).position), RUISDevice.Kinect_2); // tempVector = torsoPosition + offset * (spineBasePosition - torsoPosition) tempVector = skeletons [kinect2SensorID, playerID].torso.position + torsoOffsetKinect2 * (tempVector - skeletons[kinect2SensorID, playerID].torso.position); skeletons[kinect2SensorID, playerID].root.position = tempVector; skeletons[kinect2SensorID, playerID].torso.position = tempVector; UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineMid], body.JointOrientations[Kinect.JointType.SpineMid]), playerID, ref skeletons [kinect2SensorID, playerID].midSpine); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.SpineShoulder], body.JointOrientations[Kinect.JointType.SpineShoulder]), playerID, ref skeletons [kinect2SensorID, playerID].shoulderSpine); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ShoulderLeft], body.JointOrientations[Kinect.JointType.ShoulderLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftShoulder); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ShoulderRight], body.JointOrientations[Kinect.JointType.ShoulderRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightShoulder); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ElbowRight], body.JointOrientations[Kinect.JointType.ElbowRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightElbow); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ElbowLeft], body.JointOrientations[Kinect.JointType.ElbowLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftElbow); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandRight], body.JointOrientations[Kinect.JointType.HandRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHand); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandLeft], body.JointOrientations[Kinect.JointType.HandLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHand); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HipLeft], body.JointOrientations[Kinect.JointType.HipLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHip); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HipRight], body.JointOrientations[Kinect.JointType.HipRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHip); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandTipRight], body.JointOrientations[Kinect.JointType.HandTipRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHandTip); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandTipLeft], body.JointOrientations[Kinect.JointType.HandTipLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHandTip); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.KneeRight], body.JointOrientations[Kinect.JointType.KneeRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightKnee); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.KneeLeft], body.JointOrientations[Kinect.JointType.KneeLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftKnee); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.WristLeft], body.JointOrientations[Kinect.JointType.WristLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftWrist); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.WristRight], body.JointOrientations[Kinect.JointType.WristRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightWrist); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.AnkleLeft], body.JointOrientations[Kinect.JointType.AnkleLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftAnkle); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.AnkleRight], body.JointOrientations[Kinect.JointType.AnkleRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightAnkle); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandLeft], body.JointOrientations[Kinect.JointType.HandLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftHand); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.HandRight], body.JointOrientations[Kinect.JointType.HandRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightHand); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ThumbLeft], body.JointOrientations[Kinect.JointType.ThumbLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftThumb); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.ThumbRight], body.JointOrientations[Kinect.JointType.ThumbRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightThumb); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.FootLeft], body.JointOrientations[Kinect.JointType.FootLeft]), playerID, ref skeletons [kinect2SensorID, playerID].leftFoot); UpdateKinect2JointData(GetKinect2JointData(body.Joints[Kinect.JointType.FootRight], body.JointOrientations[Kinect.JointType.FootRight]), playerID, ref skeletons [kinect2SensorID, playerID].rightFoot); /* * Rotation corrections * Map rotations to Kinect 1 space (which is currently assumed by every script that uses RUISSkeletonManager) * HACK: Below joints are accessed from top to bottom in hierarchy, because we modify some values before using them later: * E.g. shoulder rotations are modified first because they are used to calculate lower arm rotations (same with hands and thumbs). * Also funky reassignments: skeleton[...].leftHip.rotation = skeleton[...].leftKnee.rotation * TODO: These should be fixed to be less confusing and error-inducing, preferably already in above UpdateKinect2JointData() assignments */ // *** TODO: Check that avatar rotations match that of Kinect 2 demo with boxes as body parts. Not sure about shoulders, hands, and thumbs // Upper arm 45 degree rotation ?? // Head relativePos = skeletons [kinect2SensorID, playerID].head.position - skeletons [kinect2SensorID, playerID].neck.position; skeletons [kinect2SensorID, playerID].head.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, -90, -90); // Torso relativePos = skeletons [kinect2SensorID, playerID].midSpine.position - skeletons [kinect2SensorID, playerID].shoulderSpine.position; skeletons [kinect2SensorID, playerID].torso.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, 90, -90); // TODO: Bug when turning right // Upper leg skeletons [kinect2SensorID, playerID].leftHip.rotation = skeletons [kinect2SensorID, playerID].leftKnee.rotation * Quaternion.Euler(180, -90, 0); skeletons [kinect2SensorID, playerID].rightHip.rotation = skeletons [kinect2SensorID, playerID].rightKnee.rotation * Quaternion.Euler(180, 90, 0);; // Lower leg relativePos = skeletons [kinect2SensorID, playerID].leftAnkle.position - skeletons [kinect2SensorID, playerID].leftKnee.position; skeletons [kinect2SensorID, playerID].leftKnee.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, 90, -90); // skeletons [kinect2SensorID, playerID].leftKnee.rotation = skeletons [kinect2SensorID, playerID].leftAnkle.rotation * Quaternion.Euler(180, -90, 0); relativePos = skeletons [kinect2SensorID, playerID].rightAnkle.position - skeletons [kinect2SensorID, playerID].rightKnee.position; skeletons [kinect2SensorID, playerID].rightKnee.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].midSpine.rotation * Vector3.right) * Quaternion.Euler(0, 90, -90); // skeletons [kinect2SensorID, playerID].rightKnee.rotation = skeletons [kinect2SensorID, playerID].rightAnkle.rotation * Quaternion.Euler(180, 90, 0);; // Feet / Ankles relativePos = skeletons [kinect2SensorID, playerID].leftAnkle.position - skeletons [kinect2SensorID, playerID].leftFoot.position; skeletons [kinect2SensorID, playerID].leftFoot.rotation = Quaternion.LookRotation(relativePos) * Quaternion.Euler(0, 180, 0); relativePos = skeletons [kinect2SensorID, playerID].rightAnkle.position - skeletons [kinect2SensorID, playerID].rightFoot.position; skeletons [kinect2SensorID, playerID].rightFoot.rotation = Quaternion.LookRotation(relativePos) * Quaternion.Euler(0, 180, 0); // Upper arm //relativePos = skeletons [kinect2SensorID, playerID].leftElbow.position - skeletons [kinect2SensorID, playerID].leftShoulder.position; //skeletons [kinect2SensorID, playerID].leftShoulder.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].leftElbow.rotation * Vector3.right) * Quaternion.Euler(-45, 90, 0); skeletons [kinect2SensorID, playerID].leftShoulder.rotation = skeletons [kinect2SensorID, playerID].leftElbow.rotation * Quaternion.Euler(0, 45, -90); //relativePos = skeletons [kinect2SensorID, playerID].rightElbow.position - skeletons [kinect2SensorID, playerID].rightShoulder.position; //skeletons [kinect2SensorID, playerID].rightShoulder.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].rightElbow.rotation * Vector3.right) * Quaternion.Euler(135, 270, 0); skeletons [kinect2SensorID, playerID].rightShoulder.rotation = skeletons [kinect2SensorID, playerID].rightElbow.rotation * Quaternion.Euler(0, -45, 90); // Lower arm relativePos = skeletons [kinect2SensorID, playerID].leftElbow.position - skeletons [kinect2SensorID, playerID].leftWrist.position; skeletons [kinect2SensorID, playerID].leftElbow.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].leftShoulder.rotation * Vector3.forward) * Quaternion.Euler(180, -90, 0); //skeletons [kinect2SensorID, playerID].leftElbow.rotation = skeletons [kinect2SensorID, playerID].leftWrist.rotation * Quaternion.Euler(0, 180, -90); relativePos = skeletons [kinect2SensorID, playerID].rightElbow.position - skeletons [kinect2SensorID, playerID].rightWrist.position; skeletons [kinect2SensorID, playerID].rightElbow.rotation = Quaternion.LookRotation(relativePos, skeletons [kinect2SensorID, playerID].rightShoulder.rotation * Vector3.forward) * Quaternion.Euler(180, 90, 0); //skeletons [kinect2SensorID, playerID].rightElbow.rotation = skeletons [kinect2SensorID, playerID].rightWrist.rotation * Quaternion.Euler(0, -180, 90); // Hands skeletons [kinect2SensorID, playerID].leftHand.rotation *= Quaternion.Euler(0, 180, -90); skeletons [kinect2SensorID, playerID].rightHand.rotation *= Quaternion.Euler(0, 180, 90); // Thumbs relativePos = skeletons [kinect2SensorID, playerID].leftThumb.position - skeletons[kinect2SensorID, playerID].leftWrist.position; skeletons [kinect2SensorID, playerID].leftThumb.rotation = Quaternion.LookRotation(relativePos, skeletons[kinect2SensorID, playerID].leftHand.rotation * Vector3.up) * Quaternion.Euler(-90, 0, skeletons[kinect2SensorID, playerID].thumbZRotationOffset); relativePos = skeletons [kinect2SensorID, playerID].rightThumb.position - skeletons[kinect2SensorID, playerID].rightWrist.position; skeletons [kinect2SensorID, playerID].rightThumb.rotation = Quaternion.LookRotation(relativePos, skeletons[kinect2SensorID, playerID].rightHand.rotation * Vector3.up) * Quaternion.Euler(-90, 0, -skeletons[kinect2SensorID, playerID].thumbZRotationOffset); // Fist curling switch (body.HandLeftState) { case Kinect.HandState.Closed: skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.closed; break; case Kinect.HandState.Open: skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.open; break; case Kinect.HandState.Lasso: skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.pointing; break; case Kinect.HandState.Unknown: case Kinect.HandState.NotTracked: skeletons [kinect2SensorID, playerID].leftHandStatus = RUISSkeletonManager.Skeleton.handState.unknown; break; } switch (body.HandRightState) { case Kinect.HandState.Closed: skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.closed; break; case Kinect.HandState.Open: skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.open; break; case Kinect.HandState.Lasso: skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.pointing; break; case Kinect.HandState.Unknown: case Kinect.HandState.NotTracked: skeletons [kinect2SensorID, playerID].rightHandStatus = RUISSkeletonManager.Skeleton.handState.unknown; break; } // HACK for filtering Kinect 2 arm rotations // TODO: More efficient rotation filtering, extend filtering to all joints (now just arms) if (isNewKinect2Frame && skeletons[kinect2SensorID, playerID].filterRotations) { float kalmanDeltaTime = 0.033f; //if(skeletons[kinect2SensorID, playerID].leftShoulder.rotationConfidence >= 0.5f) { filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftShoulder, 0, kalmanDeltaTime); } //if(skeletons[kinect2SensorID, playerID].rightShoulder.rotationConfidence >= 0.5f) { filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightShoulder, 1, kalmanDeltaTime); } //if(skeletons[kinect2SensorID, playerID].leftElbow.rotationConfidence >= 0.5f) { filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftElbow, 2, kalmanDeltaTime); } //if(skeletons[kinect2SensorID, playerID].rightElbow.rotationConfidence >= 0.5f) { filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightElbow, 3, kalmanDeltaTime); } // filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].root, 4, kalmanDeltaTime); // //// measuredRotation = skeletons[kinect2SensorID, playerID].root.rotation; //// skeletons[kinect2SensorID, playerID].root.rotation = skeletons[kinect2SensorID, playerID].filterRot[4].Update(measuredRotation, kalmanDeltaTime); // // TODO uncomment // filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].torso, 5, kalmanDeltaTime); // //// measuredRotation = skeletons[kinect2SensorID, playerID].torso.rotation; //// skeletons[kinect2SensorID, playerID].torso.rotation = skeletons[kinect2SensorID, playerID].filterRot[5].Update(measuredRotation, kalmanDeltaTime); // // TODO uncomment // filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].midSpine, 6, kalmanDeltaTime); // //// measuredRotation = skeletons[kinect2SensorID, playerID].midSpine.rotation; //// skeletons[kinect2SensorID, playerID].midSpine.rotation = skeletons[kinect2SensorID, playerID].filterRot[6].Update(measuredRotation, kalmanDeltaTime); // // TODO uncomment // filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].shoulderSpine, 7, kalmanDeltaTime); // //// measuredRotation = skeletons[kinect2SensorID, playerID].shoulderSpine.rotation; //// skeletons[kinect2SensorID, playerID].shoulderSpine.rotation = skeletons[kinect2SensorID, playerID].filterRot[7].Update(measuredRotation, kalmanDeltaTime); filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftHand, 8, kalmanDeltaTime); // measuredRotation = skeletons[kinect2SensorID, playerID].leftHand.rotation; // skeletons[kinect2SensorID, playerID].leftHand.rotation = skeletons[kinect2SensorID, playerID].filterRot[8].Update(measuredRotation, kalmanDeltaTime); filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightHand, 9, kalmanDeltaTime); filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightHip, 10, kalmanDeltaTime); filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftHip, 11, kalmanDeltaTime); filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].rightKnee, 12, kalmanDeltaTime); filterJointRotation(ref skeletons[kinect2SensorID, playerID], ref skeletons[kinect2SensorID, playerID].leftKnee, 13, kalmanDeltaTime); // measuredRotation = skeletons[kinect2SensorID, playerID].rightHand.rotation; // skeletons[kinect2SensorID, playerID].rightHand.rotation = skeletons[kinect2SensorID, playerID].filterRot[9].Update(measuredRotation, kalmanDeltaTime); // Kalman filtering is a slightly heavy operation, just average two latest thumb rotations // TODO uncomment // Quaternion thumbRotation = skeletons[kinect2SensorID, playerID].leftThumb.rotation; // skeletons[kinect2SensorID, playerID].leftThumb.rotation = Quaternion.Slerp(thumbRotation, // skeletons[kinect2SensorID, playerID].previousRotation[10], 0.5f); // skeletons[kinect2SensorID, playerID].previousRotation[10] = thumbRotation; // // thumbRotation = skeletons[kinect2SensorID, playerID].rightThumb.rotation; // skeletons[kinect2SensorID, playerID].rightThumb.rotation = Quaternion.Slerp(thumbRotation, // skeletons[kinect2SensorID, playerID].previousRotation[11], 0.5f); // skeletons[kinect2SensorID, playerID].previousRotation[11] = thumbRotation; } i++; } } } if (isNewKinect2Frame) { kinect2FrameDeltaT = timeSinceLastKinect2Frame; timeSinceLastKinect2Frame = 0; } else { timeSinceLastKinect2Frame += Time.deltaTime; } } }
private Vector3 GetPivotPositionInTrackerCoordinates() { if (useOculusPositionalTracking && OVRManager.tracker != null && OVRManager.tracker.isPositionTracked) { if (coordinateSystem && coordinateSystem.applyToRootCoordinates) { return(coordinateSystem.ConvertLocation(coordinateSystem.GetOculusRiftLocation(), RUISDevice.Oculus_DK2)); } else if (OVRManager.display != null) { return(OVRManager.display.GetHeadPose().position); } } else { switch (characterPivotType) { case CharacterPivotType.KinectHead: { if ((bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID && !inputManager.enableKinect2) || (bodyTrackingDeviceID == RUISSkeletonManager.kinect1SensorID && !inputManager.enableKinect)) { break; } if (skeletonManager && skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId] != null) { if (skeletonController) // Add root speed scaling { return(Vector3.Scale(skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId].head.position, skeletonController.rootSpeedScaling)); } else { return(skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId].head.position); } } break; } case CharacterPivotType.KinectTorso: { if ((bodyTrackingDeviceID == RUISSkeletonManager.kinect2SensorID && !inputManager.enableKinect2) || (bodyTrackingDeviceID == RUISSkeletonManager.kinect1SensorID && !inputManager.enableKinect)) { break; } if (skeletonManager && skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId] != null) { if (skeletonController) // Add root speed scaling { return(Vector3.Scale(skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId].torso.position, skeletonController.rootSpeedScaling)); } else { return(skeletonManager.skeletons[bodyTrackingDeviceID, kinectPlayerId].torso.position); } } break; } case CharacterPivotType.MoveController: { if (!inputManager.enablePSMove) { break; } if (inputManager.GetMoveWand(moveControllerId)) { return(inputManager.GetMoveWand(moveControllerId).handlePosition); } break; } } } if (skeletonController != null) { return(skeletonController.transform.localPosition); } else { return(Vector3.zero); } }
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; * */ }
private Vector3 TransformPosition(Vector3 value) { return(coordinateSystem.ConvertLocation(coordinateSystem.ConvertRawPSMoveLocation(value), RUISDevice.PS_Move)); }