//-------------------------------------------------------------------------------------- // Implementation of a Holt Double Exponential Smoothing filter. The double exponential // smooths the curve and predicts. There is also noise jitter removal. And maximum // prediction bounds. The paramaters are commented in the init function. //-------------------------------------------------------------------------------------- public void Update(Kinect.Body body) { if (body == null) { return; } // Check for divide by zero. Use an epsilon of a 10th of a millimeter smoothParams.fJitterRadius = Mathf.Max(0.0001f, smoothParams.fJitterRadius); TRANSFORM_SMOOTH_PARAMETERS smoothingParams; foreach (Kinect.JointType jt in Kinect.JointType.GetValues(typeof(Kinect.JointType))) { smoothingParams.fSmoothing = smoothParams.fSmoothing; smoothingParams.fCorrection = smoothParams.fCorrection; smoothingParams.fPrediction = smoothParams.fPrediction; smoothingParams.fJitterRadius = smoothParams.fJitterRadius; smoothingParams.fMaxDeviationRadius = smoothParams.fMaxDeviationRadius; // If inferred, we smooth a bit more by using a bigger jitter radius Windows.Kinect.Joint joint = body.Joints[jt]; if (joint.TrackingState == Kinect.TrackingState.Inferred) { smoothingParams.fJitterRadius *= 2.0f; smoothingParams.fMaxDeviationRadius *= 2.0f; } // set initial joint value from Kinect Joint fj = new Joint(body.Joints[jt].Position, body.JointOrientations[jt].Orientation); UpdateJoint(jt, fj, smoothingParams); } }
private void UpdateJoint(Joint joint, Kinect.Body body, Vector3 cameraPosition, Quaternion cameraRotation) { if (joint == null || body == null) { return; } Kinect.JointType?jt = Helpers.ParseEnum <Kinect.JointType>(joint.Name); // get Kinects raw value and filter it if (jt != null) { // If inferred, we smooth a bit more by using a bigger jitter radius Windows.Kinect.Joint kinectJoint = body.Joints[jt.Value]; if (kinectJoint.TrackingState == Kinect.TrackingState.Inferred) { this.smoothingParams.fJitterRadius *= 2.0f; this.smoothingParams.fMaxDeviationRadius *= 2.0f; } // get the initial joint value from Kinect, correct for camera Vector3 rawPosition = ConvertJointPositionToUnityVector3(body, jt.Value, this.MirrorJoints); Quaternion rawRotation = ConvertJointQuaternionToUnityQuaterion(body, jt.Value, this.MirrorJoints); // visualize the unadjusted joint information // turn on Gizmos if you want to see these in Game mode // Helpers.DrawDebugQuaternion(rawPosition, rawRotation, Helpers.ColorRange.BW, .05f); // adjust for camera rawPosition = cameraRotation * rawPosition; rawRotation = cameraRotation * rawRotation; // if this is a leaf bone, with no orientation if (Helpers.QuaternionZero.Equals(rawRotation) && joint.Parent != null) { Vector3 direction = rawPosition - joint.Parent.RawPosition; Vector3 perpendicular = Vector3.Cross(direction, Vector3.up); Vector3 normal = Vector3.Cross(perpendicular, direction); // calculate a rotation, Y forward for Kinect if (normal.magnitude != 0) { rawRotation = Quaternion.LookRotation(normal, direction); } else { rawRotation = Quaternion.identity; } } // set the filtered joint property with the updates values this.filteredJoint.Position = rawPosition; this.filteredJoint.Rotation = rawRotation; // filter the raw joint this.filteredJoint = jointSmoother.UpdateJoint(jt.Value, this.filteredJoint, smoothingParams); // set the raw joint value for the node joint.SetRawData(this.filteredJoint.Position, this.filteredJoint.Rotation); } // calculate offsets from world position/rotation joint.CalculateOffset(cameraPosition, cameraRotation); // continue through hierarchy if (joint.Children != null) { foreach (var child in joint.Children) { UpdateJoint(child, body, cameraPosition, cameraRotation); } } }
private void UpdateJoints(Kinect.Body body) { if (body == null) { return; } DoubleExponentialFilter.TRANSFORM_SMOOTH_PARAMETERS smoothingParams = jointSmoother.SmoothingParameters; // get the floorClipPlace from the body information Vector4 floorClipPlane = Helpers.FloorClipPlane; // get rotation of camera Quaternion cameraRotation = Helpers.CalculateFloorRotationCorrection(floorClipPlane); // generate a vertical offset from floor plane Vector3 floorOffset = cameraRotation * Vector3.up * floorClipPlane.w; foreach (Kinect.JointType jt in Enum.GetValues(typeof(Kinect.JointType))) { // If inferred, we smooth a bit more by using a bigger jitter radius Windows.Kinect.Joint kinectJoint = body.Joints[jt]; if (kinectJoint.TrackingState == Kinect.TrackingState.Inferred) { smoothingParams.fJitterRadius *= 2.0f; smoothingParams.fMaxDeviationRadius *= 2.0f; } Joint parent = this.Joints[jt].Parent; // set initial joint value from Kinect Vector3 rawPosition = cameraRotation * ConvertJointPositionToUnityVector3(body, jt) + floorOffset; Quaternion rawRotation = cameraRotation * ConvertJointQuaternionToUnityQuaterion(body, jt); if (Helpers.QuaternionZero.Equals(rawRotation) && parent != null) { Vector3 direction = rawPosition - parent.RawPosition; Vector3 perpendicular = Vector3.Cross(direction, Vector3.up); Vector3 normal = Vector3.Cross(perpendicular, direction); // calculate a rotation, Y forward for Kinect if (normal.magnitude != 0) { rawRotation = Quaternion.LookRotation(normal, direction); } else { rawRotation = Quaternion.identity; } } DoubleExponentialFilter.Joint fj = new DoubleExponentialFilter.Joint(rawPosition, rawRotation); fj = jointSmoother.UpdateJoint(jt, fj, smoothingParams); // set the raw joint value for the node this.Joints[jt].SetRawtData(fj.Position, fj.Rotation); } //Vector3 offsetPosition = this.Joints[Kinect.JointType.SpineBase].RawPosition - floorOffset; Vector3 offsetPosition = Vector3.zero; Quaternion offsetRotation = Quaternion.identity; // calculate the relative joint and rotation this.Joints[Kinect.JointType.SpineBase].CalculateOffset(offsetPosition, offsetRotation); }
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] }