private static Color GetColorForState(Kinect.TrackingState state) { switch (state) { case Kinect.TrackingState.Tracked: return(Color.green); case Kinect.TrackingState.Inferred: return(Color.red); default: return(Color.black); } }
private static int GetInferedState(Kinect.TrackingState state) { switch (state) { case Kinect.TrackingState.Tracked: return(0); case Kinect.TrackingState.Inferred: return(1); default: return(1); } }
private static Color GetColorForState(Kinect.TrackingState state) { //switch (state) //{ //case Kinect.TrackingState.Tracked: // return Color.green; //case Kinect.TrackingState.Inferred: return(Color.red); //default: // return Color.black; //} }
void CheckMainBody(Kinect.Body[] bodies) { // There is a chance that there will be no body avaliable if (bodies.Length <= m_MainCameraIndex) { return; } // Zero is always the main body for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++) { Kinect.TrackingState trackingState = bodies[m_MainCameraIndex].Joints[jt].TrackingState; Kinect.Joint sourceJoint = bodies[m_MainCameraIndex].Joints[jt]; GameObject jtObj = JointToGameObject(jt); if (jtObj) { if (jtObj.GetComponent <Renderer>()) { jtObj.GetComponent <Renderer>().enabled = !hideLocal; } if (jtObj.GetComponent <LineRenderer>()) { jtObj.GetComponent <LineRenderer>().enabled = !hideLocal; } } if (sourceJoint.TrackingState != Kinect.TrackingState.Tracked && bodies.Length > 1) // When there is only one body avalible (aka. only one camera tracking), do not try to use only tracked data { // Try replace it with tracked backups for (int n = 0; n < bodies.Length; n++) { if (n == m_MainCameraIndex) { continue; } Kinect.Joint backupJoint = bodies[n].Joints[Kinect.JointMap._MirrorBoneMap[jt]]; if (backupJoint.TrackingState == Kinect.TrackingState.Tracked) { // We can try this trackingState = backupJoint.TrackingState; Vector3 jointPosition = getPosition(m_Cameras[n].JointToGameObject(Kinect.JointMap._MirrorBoneMap[jt]).transform.position); Quaternion jointRotation = m_Cameras[n].JointToGameObject(Kinect.JointMap._MirrorBoneMap[jt]).transform.rotation; FusedBody.UpdateJoint(jt, jointPosition, jointRotation, trackingState); if (jtObj) { jtObj.transform.localPosition = getPosition(m_Cameras[n].JointToGameObject(Kinect.JointMap._MirrorBoneMap[jt]).transform.position); jtObj.transform.localRotation = m_Cameras[n].JointToGameObject(Kinect.JointMap._MirrorBoneMap[jt]).transform.rotation; } } } } else { Vector3 jointPosition = getPosition(m_Cameras[m_MainCameraIndex].JointToGameObject(jt).transform.position); Quaternion jointRotation = m_Cameras[m_MainCameraIndex].JointToGameObject(jt).transform.rotation; FusedBody.UpdateJoint(jt, jointPosition, jointRotation, trackingState); if (jtObj) { jtObj.transform.localPosition = getPosition(m_Cameras[m_MainCameraIndex].JointToGameObject(jt).transform.position); jtObj.transform.localRotation = m_Cameras[m_MainCameraIndex].JointToGameObject(jt).transform.rotation; } } if (Kinect.JointMap._BoneMap.ContainsKey(jt) && jtObj) { LineRenderer lr = jtObj.GetComponent <LineRenderer>(); if (lr) { lr.SetPosition(0, jtObj.transform.position); lr.SetPosition(1, JointToGameObject(Kinect.JointMap._BoneMap[jt]).transform.position); // Update Rotation as well if (Kinect.JointMap._RadialBoneMap.ContainsKey(jt)) { jtObj.transform.rotation = Quaternion.LookRotation((JointToGameObject(Kinect.JointMap._RadialBoneMap[jt]).transform.position - jtObj.transform.position).normalized); } } } } for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++) { if (jt == Kinect.JointType.SpineBase) { RelativeFusedBody.UpdateJoint(jt, FusedBody.Joints[jt].Position, FusedBody.JointOrientations[jt].Orientation, FusedBody.Joints[jt].TrackingState); } else if (Kinect.JointMap._RadialBoneMap.ContainsKey(jt)) { RelativeFusedBody.UpdateJoint(jt, FusedBody.Joints[jt].Position - FusedBody.Joints[Kinect.JointMap._RadialBoneMap[jt]].Position, Quaternion.identity, FusedBody.Joints[jt].TrackingState); } } }
public void JointUpdate(Dictionary <Kinect.JointType, Kinect.Joint> joints, Dictionary <Kinect.JointType, Kinect.JointOrientation> orients, int JointID, Kinect.JointType type, Kinect.TrackingState state, TRANSFORM_SMOOTH_PARAMETERS smoothingParams, int body_id = 0) { //printf("body: %d\n", body_id); Vector3 vPrevRawPosition; Vector3 vPrevFilteredPosition; Vector3 vPrevTrend; Vector3 vRawPosition; Vector3 vFilteredPosition; Vector3 vPredictedPosition; Vector3 vDiff; Vector3 vTrend; float vLength; float fDiff; bool bJointIsValid; Quaternion filteredOrientation; Quaternion trend; Quaternion rawOrientation = new Quaternion(orients[(Kinect.JointType)JointID].Orientation.X, orients[(Kinect.JointType)JointID].Orientation.Y, orients[(Kinect.JointType)JointID].Orientation.Z, orients[(Kinect.JointType)JointID].Orientation.W); Quaternion prevFilteredOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation; Quaternion prevTrend = m_pHistory[body_id, JointID].m_vTrendOrientation; Kinect.Joint joint = joints[(Kinect.JointType)JointID]; Kinect.JointOrientation orient = orients[(Kinect.JointType)JointID]; vRawPosition = new Vector4(joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f); vPrevFilteredPosition = m_pHistory[body_id, JointID].m_vFilteredPosition; vPrevTrend = m_pHistory[body_id, JointID].m_vTrend; vPrevRawPosition = m_pHistory[body_id, JointID].m_vRawPosition; bJointIsValid = JointPositionIsValid(vRawPosition); // If joint is invalid, reset the filter if (!bJointIsValid) { rawOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation; m_pHistory[body_id, JointID].m_dwFrameCount = 0; } // Initial start values if (m_pHistory[body_id, JointID].m_dwFrameCount == 0) { vFilteredPosition = vRawPosition; vTrend = Vector3.zero; m_pHistory[body_id, JointID].m_dwFrameCount++; filteredOrientation = rawOrientation; trend = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f); } else if (m_pHistory[body_id, JointID].m_dwFrameCount == 1) { vFilteredPosition = (vRawPosition + vPrevRawPosition) * 0.5f; vDiff = vFilteredPosition - vPrevFilteredPosition; vTrend = (vDiff * smoothingParams.fCorrection) + vPrevTrend * (1.0f - smoothingParams.fCorrection); m_pHistory[body_id, JointID].m_dwFrameCount++; Quaternion prevRawOrientation = m_pHistory[body_id, JointID].m_vRawOrientation; filteredOrientation = EnhansedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f); Quaternion diffStarted = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = EnhansedQuaternionSlerp(prevTrend, diffStarted, smoothingParams.fCorrection); } else { // First apply jitter filter vDiff = vRawPosition - vPrevFilteredPosition; vLength = Mathf.Sqrt(vDiff.sqrMagnitude); fDiff = Mathf.Abs(vLength); if (fDiff <= smoothingParams.fJitterRadius) { vFilteredPosition = vRawPosition * (fDiff / smoothingParams.fJitterRadius) + vPrevFilteredPosition * (1.0f - fDiff / smoothingParams.fJitterRadius); } else { vFilteredPosition = vRawPosition; } Quaternion diffJitter = RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation); float diffValJitter = Mathf.Abs(QuaternionAngle(diffJitter)); if (diffValJitter <= smoothingParams.fJitterRadius) { filteredOrientation = EnhansedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParams.fJitterRadius); } else { filteredOrientation = rawOrientation; } // Now the double exponential smoothing filter vFilteredPosition = vFilteredPosition * (1.0f - smoothingParams.fSmoothing) + (vPrevFilteredPosition + vPrevTrend) * smoothingParams.fSmoothing; vDiff = vFilteredPosition - vPrevFilteredPosition; vTrend = ((vDiff * smoothingParams.fCorrection) + (vPrevTrend * (1.0f - smoothingParams.fCorrection))); // Now the double exponential smoothing filter filteredOrientation = EnhansedQuaternionSlerp(filteredOrientation, (prevFilteredOrientation * prevTrend), smoothingParams.fSmoothing); diffJitter = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = EnhansedQuaternionSlerp(prevTrend, diffJitter, smoothingParams.fCorrection); } // Predict into the future to reduce latency vPredictedPosition = (vFilteredPosition + (vTrend * smoothingParams.fPrediction)); Quaternion predictedOrientation = (filteredOrientation * (EnhansedQuaternionSlerp(new Quaternion(0.0f, 0.0f, 0.0f, 1.0f), trend, smoothingParams.fPrediction))); // Check that we are not too far away from raw data vDiff = (vRawPosition - vPrevFilteredPosition); vLength = Mathf.Sqrt(vDiff.sqrMagnitude); fDiff = Mathf.Abs(vLength); Quaternion diff = RotationBetweenQuaternions(predictedOrientation, filteredOrientation); float diffVal = Mathf.Abs(QuaternionAngle(diff)); if (fDiff > smoothingParams.fMaxDeviationRadius) { vPredictedPosition = ((vPredictedPosition * (smoothingParams.fMaxDeviationRadius / fDiff)) + (vRawPosition * (1.0f - smoothingParams.fMaxDeviationRadius / fDiff))); } if (diffVal > smoothingParams.fMaxDeviationRadius) { predictedOrientation = EnhansedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParams.fMaxDeviationRadius / diffVal); } predictedOrientation = QuaternionNormalise(predictedOrientation); filteredOrientation = QuaternionNormalise(filteredOrientation); trend = QuaternionNormalise(trend); // Save the data from this frame m_pHistory[body_id, JointID].m_vRawPosition = vRawPosition; m_pHistory[body_id, JointID].m_vFilteredPosition = vFilteredPosition; m_pHistory[body_id, JointID].m_vTrend = vTrend; if (!(rawOrientation == null) && !(filteredOrientation == null) && !(trend == null)) { m_pHistory[body_id, JointID].m_vRawOrientation = rawOrientation; m_pHistory[body_id, JointID].m_vFilteredOrientation = filteredOrientation; m_pHistory[body_id, JointID].m_vTrendOrientation = trend; } // Output the data //vPredictedPosition = new Vector4(vPredictedPosition , 1.0f); Kinect.Joint j = new Windows.Kinect.Joint(); Kinect.CameraSpacePoint CameraSpacePoint = new Windows.Kinect.CameraSpacePoint(); CameraSpacePoint.X = vPredictedPosition.x; CameraSpacePoint.Y = vPredictedPosition.y; CameraSpacePoint.Z = vPredictedPosition.z; j.Position = CameraSpacePoint; j.JointType = type; j.TrackingState = state; m_pFilteredJoints[(Kinect.JointType)JointID] = j; // HipCenter has no parent and is the root of our skeleton - leave the HipCenter absolute set as it is if (type != Kinect.JointType.SpineBase) { Kinect.JointOrientation jo = new Windows.Kinect.JointOrientation(); Kinect.Vector4 v4 = new Kinect.Vector4(); v4.X = predictedOrientation.x; v4.Y = predictedOrientation.y; v4.Z = predictedOrientation.z; v4.W = predictedOrientation.w; jo.Orientation = v4; jo.JointType = type; m_pFilteredOrientations[(Kinect.JointType)JointID] = jo; } else { m_pFilteredOrientations[(Kinect.JointType)JointID] = orients[(Kinect.JointType)JointID]; } //m_pFilteredJoints[JointID] }
void onKinectDataReceived(PsychoFrameListener sender, string identifier, int bodyIndex, Kinect.JointType jointType, Vector3 position, Quaternion rotation, Kinect.TrackingState trackingState) { if (identifier != this.identifier && this.identifier != "") { return; } if (!_AvaliableBody.ContainsKey(bodyIndex)) { _AvaliableBody[bodyIndex] = new Kinect.Body(); } _AvaliableBody[bodyIndex].UpdateJoint(jointType, position, rotation, trackingState); //print(identifier + ": " + bodyIndex + ": " + jointType.ToString() + ": " + position.ToString() + " " + rotation.ToString() + " " + trackingState.ToString()); }