public Joint UpdateJoint(Kinect.JointType jt, Joint rawJoint, TRANSFORM_SMOOTH_PARAMETERS smoothingParams) { Joint prevFilteredJoint; Joint prevRawJoint; Joint prevTrend; Joint filteredJoint; Joint predictedJoint; Joint diff; Joint trend; float fDiff; prevFilteredJoint = history[jt].FilteredJoint; prevTrend = history[jt].Trend; prevRawJoint = history[jt].RawJoint; // if joint is invalid, reset the filter bool jointIsValid = JointPositionIsValid(rawJoint); if (!jointIsValid) { history[jt].FrameCount = 0; } // initial start values if (history[jt].FrameCount == 0) { filteredJoint = rawJoint; trend = new Joint(); history[jt].FrameCount++; } else if (history[jt].FrameCount == 1) { filteredJoint = Joint.Scale(Joint.Add(rawJoint, prevRawJoint), 0.5f); diff = Joint.Subtract(filteredJoint, prevFilteredJoint); trend = Joint.Add(Joint.Scale(diff, smoothingParams.fCorrection), Joint.Scale(prevTrend, 1.0f - smoothingParams.fCorrection)); history[jt].FrameCount++; } else { // First apply jitter filter diff = Joint.Subtract(rawJoint, prevFilteredJoint); fDiff = diff.Position.magnitude; if (fDiff <= smoothingParams.fJitterRadius) { filteredJoint = Joint.Add(Joint.Scale(rawJoint, fDiff / smoothingParams.fJitterRadius), Joint.Scale(prevFilteredJoint, 1.0f - fDiff / smoothingParams.fJitterRadius)); } else { filteredJoint = rawJoint; } // Now the double exponential smoothing filter filteredJoint = Joint.Add(Joint.Scale(filteredJoint, 1.0f - smoothingParams.fSmoothing), Joint.Scale(Joint.Add(prevFilteredJoint, prevTrend), smoothingParams.fSmoothing)); diff = Joint.Subtract(filteredJoint, prevFilteredJoint); trend = Joint.Add(Joint.Scale(diff, smoothingParams.fCorrection), Joint.Scale(prevTrend, 1.0f - smoothingParams.fCorrection)); } // Predict into the future to reduce latency predictedJoint = Joint.Add(filteredJoint, Joint.Scale(trend, smoothingParams.fPrediction)); // Check that we are not too far away from raw data diff = Joint.Subtract(predictedJoint, rawJoint); fDiff = diff.Position.magnitude; if (fDiff > smoothingParams.fMaxDeviationRadius) { predictedJoint = Joint.Add(Joint.Scale(predictedJoint, smoothingParams.fMaxDeviationRadius / fDiff), Joint.Scale(rawJoint, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff)); } // Save the data from this frame history[jt].RawJoint = rawJoint; history[jt].FilteredJoint = filteredJoint; history[jt].Trend = trend; // Output the data filteredJoints[jt] = predictedJoint; return(predictedJoint); }
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams) { CameraSpacePoint vPrevRawPosition; CameraSpacePoint vPrevFilteredPosition; CameraSpacePoint vPrevTrend; CameraSpacePoint vRawPosition; CameraSpacePoint vFilteredPosition; CameraSpacePoint vPredictedPosition; CameraSpacePoint vDiff; CameraSpacePoint vTrend; float fDiff; bool bJointIsValid; Joint joint = body.Joints[jt]; vRawPosition = joint.Position; vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition; vPrevTrend = m_pHistory[(int)jt].m_vTrend; vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition; bJointIsValid = JointPositionIsValid(vRawPosition); // If joint is invalid, reset the filter if (!bJointIsValid) { m_pHistory[(int)jt].m_dwFrameCount = 0; } // Initial start values if (m_pHistory[(int)jt].m_dwFrameCount == 0) { vFilteredPosition = vRawPosition; vTrend = CSVectorZero(); m_pHistory[(int)jt].m_dwFrameCount++; } else if (m_pHistory[(int)jt].m_dwFrameCount == 1) { vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f); vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition); vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection)); m_pHistory[(int)jt].m_dwFrameCount++; } else { // First apply jitter filter vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition); fDiff = CSVectorLength(vDiff); if (fDiff <= smoothingParams.fJitterRadius) { vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius), CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius)); } else { vFilteredPosition = vRawPosition; } // Now the double exponential smoothing filter vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing), CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing)); vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition); vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection)); } // Predict into the future to reduce latency vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction)); // Check that we are not too far away from raw data vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition); fDiff = CSVectorLength(vDiff); if (fDiff > smoothingParams.fMaxDeviationRadius) { vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff), CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff)); } // Save the data from this frame m_pHistory[(int)jt].m_vRawPosition = vRawPosition; m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition; m_pHistory[(int)jt].m_vTrend = vTrend; // Output the data m_pFilteredJoints[(int)jt] = vPredictedPosition; }
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] }
private void Update(FilteredTransform rawJoint, TRANSFORM_SMOOTH_PARAMETERS smoothingParams) { FilteredTransform prevFilteredJoint; FilteredTransform prevRawJoint; FilteredTransform prevTrend; FilteredTransform predictedJoint; FilteredTransform diff; FilteredTransform trend; float fDiff; prevFilteredJoint = history.FilteredJoint; prevTrend = history.Trend; prevRawJoint = history.RawJoint; // initial start values if (history.FrameCount == 0) { filteredTransform = rawJoint; trend = new FilteredTransform(); history.FrameCount++; } else if (history.FrameCount == 1) { filteredTransform = FilteredTransform.Scale(FilteredTransform.Add(rawJoint, prevRawJoint), 0.5f); diff = FilteredTransform.Subtract(filteredTransform, prevFilteredJoint); trend = FilteredTransform.Add(FilteredTransform.Scale(diff, smoothingParams.fCorrection), FilteredTransform.Scale(prevTrend, 1.0f - smoothingParams.fCorrection)); history.FrameCount++; } else { // First apply jitter filter diff = FilteredTransform.Subtract(rawJoint, prevFilteredJoint); fDiff = diff.Position.magnitude; if (fDiff <= smoothingParams.fJitterRadius) { filteredTransform = FilteredTransform.Add(FilteredTransform.Scale(rawJoint, fDiff / smoothingParams.fJitterRadius), FilteredTransform.Scale(prevFilteredJoint, 1.0f - fDiff / smoothingParams.fJitterRadius)); } else { filteredTransform = rawJoint; } // Now the double exponential smoothing filter filteredTransform = FilteredTransform.Add(FilteredTransform.Scale(filteredTransform, 1.0f - smoothingParams.fSmoothing), FilteredTransform.Scale(FilteredTransform.Add(prevFilteredJoint, prevTrend), smoothingParams.fSmoothing)); diff = FilteredTransform.Subtract(filteredTransform, prevFilteredJoint); trend = FilteredTransform.Add(FilteredTransform.Scale(diff, smoothingParams.fCorrection), FilteredTransform.Scale(prevTrend, 1.0f - smoothingParams.fCorrection)); } // Predict into the future to reduce latency predictedJoint = FilteredTransform.Add(filteredTransform, FilteredTransform.Scale(trend, smoothingParams.fPrediction)); // Check that we are not too far away from raw data diff = FilteredTransform.Subtract(predictedJoint, rawJoint); fDiff = diff.Position.magnitude; if (fDiff > smoothingParams.fMaxDeviationRadius) { predictedJoint = FilteredTransform.Add(FilteredTransform.Scale(predictedJoint, smoothingParams.fMaxDeviationRadius / fDiff), FilteredTransform.Scale(rawJoint, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff)); } // Save the data from this frame history.RawJoint = rawJoint; history.FilteredJoint = filteredTransform; history.Trend = trend; // Output the data filteredTransform = predictedJoint; }