// Initialize the filter with a set of TransformSmoothParameters. public void Init(KinectCommon.NuiTransformSmoothParameters smoothingParameters) { this.smoothParameters = smoothingParameters; this.Reset(); this.init = true; }
/// <summary> /// Initialize the filter with a set of manually specified TransformSmoothParameters. /// </summary> /// <param name="smoothingValue">Smoothing = [0..1], lower values is closer to the raw data and more noisy.</param> /// <param name="correctionValue">Correction = [0..1], higher values correct faster and feel more responsive.</param> /// <param name="predictionValue">Prediction = [0..n], how many frames into the future we want to predict.</param> /// <param name="jitterRadiusValue">JitterRadius = The deviation distance in m that defines jitter.</param> /// <param name="maxDeviationRadiusValue">MaxDeviation = The maximum distance in m that filtered positions are allowed to deviate from raw data.</param> public void Init(float smoothingValue, float correctionValue, float predictionValue, float jitterRadiusValue, float maxDeviationRadiusValue) { this.smoothParameters = new KinectCommon.NuiTransformSmoothParameters(); this.smoothParameters.fSmoothing = smoothingValue; // How much soothing will occur. Will lag when too high this.smoothParameters.fCorrection = correctionValue; // How much to correct back from prediction. Can make things springy this.smoothParameters.fPrediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high this.smoothParameters.fJitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high this.smoothParameters.fMaxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high this.Reset(); this.init = true; }
// Update the filter with a new frame of data and smooth. public void UpdateFilter(ref KinectCommon.NuiSkeletonData skeleton) { // if (null == skeleton) // { // return; // } if (skeleton.eTrackingState != KinectCommon.NuiSkeletonTrackingState.SkeletonTracked) { return; } if (this.init == false) { this.Init(); // initialize with default parameters } //Array jointTypeValues = Enum.GetValues(typeof(KinectCommon.NuiSkeletonPositionIndex)); KinectCommon.NuiTransformSmoothParameters tempSmoothingParams = new KinectCommon.NuiTransformSmoothParameters(); // Check for divide by zero. Use an epsilon of a 10th of a millimeter this.smoothParameters.fJitterRadius = Math.Max(0.0001f, this.smoothParameters.fJitterRadius); tempSmoothingParams.fSmoothing = smoothParameters.fSmoothing; tempSmoothingParams.fCorrection = smoothParameters.fCorrection; tempSmoothingParams.fPrediction = smoothParameters.fPrediction; int jointsCount = (int)KinectCommon.NuiSkeletonPositionIndex.Count; for (int jointIndex = 0; jointIndex < jointsCount; jointIndex++) { //KinectCommon.NuiSkeletonPositionIndex jt = (KinectCommon.NuiSkeletonPositionIndex)jointTypeValues.GetValue(jointIndex); // If not tracked, we smooth a bit more by using a bigger jitter radius // Always filter feet highly as they are so noisy if (skeleton.SkeletonTrackingState[jointIndex] != KinectCommon.NuiSkeletonPositionTrackingState.Tracked) { tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius * 2.0f; tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius * 2.0f; } else { tempSmoothingParams.fJitterRadius = smoothParameters.fJitterRadius; tempSmoothingParams.fMaxDeviationRadius = smoothParameters.fMaxDeviationRadius; } FilterJoint(ref skeleton, jointIndex, ref tempSmoothingParams); } }
// Update the filter for one joint. protected void FilterJoint(ref KinectCommon.NuiSkeletonData skeleton, int jointIndex, ref KinectCommon.NuiTransformSmoothParameters smoothingParameters) { float filteredState; float trend; float diffVal; float rawState = (float)skeleton.SkeletonTrackingState[jointIndex]; float prevFilteredState = history[jointIndex].FilteredState; float prevTrend = history[jointIndex].Trend; float prevRawState = history[jointIndex].RawState; // If joint is invalid, reset the filter if (rawState == 0f) { history[jointIndex].FrameCount = 0; } // Initial start values if (history[jointIndex].FrameCount == 0) { filteredState = rawState; trend = 0f; } else if (this.history[jointIndex].FrameCount == 1) { filteredState = (rawState + prevRawState) * 0.5f; diffVal = filteredState - prevFilteredState; trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } else { // // First apply jitter filter // diffVal = rawState - prevFilteredState; // // if (diffVal <= smoothingParameters.fJitterRadius) // { // filteredState = (rawState * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredState * (1.0f - (diffVal / smoothingParameters.fJitterRadius))); // } // else // { // filteredState = rawState; // } filteredState = rawState; // Now the double exponential smoothing filter filteredState = (filteredState * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredState + prevTrend) * smoothingParameters.fSmoothing); diffVal = filteredState - prevFilteredState; trend = (diffVal * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } // Predict into the future to reduce latency float predictedState = filteredState + (trend * smoothingParameters.fPrediction); // Check that we are not too far away from raw data diffVal = predictedState - rawState; if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedState = (predictedState * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawState * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal))); } // Save the data from this frame history[jointIndex].RawState = rawState; history[jointIndex].FilteredState = filteredState; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered data back into the joint skeleton.SkeletonTrackingState[jointIndex] = (KinectCommon.NuiSkeletonPositionTrackingState)(predictedState + 0.5f); }
// Update the filter for one joint. protected void FilterJoint(ref KinectCommon.NuiSkeletonData skeleton, int jointIndex, ref KinectCommon.NuiTransformSmoothParameters smoothingParameters) { // if (null == skeleton) // { // return; // } //int jointIndex = (int)jt; Vector3 filteredPosition; Vector3 diffvec; Vector3 trend; float diffVal; Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex]; Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition; Vector3 prevTrend = this.history[jointIndex].Trend; Vector3 prevRawPosition = this.history[jointIndex].RawPosition; bool jointIsValid = KinectHelper.JointPositionIsValid(rawPosition); // If joint is invalid, reset the filter if (!jointIsValid) { history[jointIndex].FrameCount = 0; } // Initial start values if (this.history[jointIndex].FrameCount == 0) { filteredPosition = rawPosition; trend = Vector3.zero; } else if (this.history[jointIndex].FrameCount == 1) { filteredPosition = (rawPosition + prevRawPosition) * 0.5f; diffvec = filteredPosition - prevFilteredPosition; trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } else { // First apply jitter filter diffvec = rawPosition - prevFilteredPosition; diffVal = Math.Abs(diffvec.magnitude); if (diffVal <= smoothingParameters.fJitterRadius) { filteredPosition = (rawPosition * (diffVal / smoothingParameters.fJitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.fJitterRadius))); } else { filteredPosition = rawPosition; } // Now the double exponential smoothing filter filteredPosition = (filteredPosition * (1.0f - smoothingParameters.fSmoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.fSmoothing); diffvec = filteredPosition - prevFilteredPosition; trend = (diffvec * smoothingParameters.fCorrection) + (prevTrend * (1.0f - smoothingParameters.fCorrection)); } // Predict into the future to reduce latency Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.fPrediction); // Check that we are not too far away from raw data diffvec = predictedPosition - rawPosition; diffVal = Mathf.Abs(diffvec.magnitude); if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedPosition = (predictedPosition * (smoothingParameters.fMaxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.fMaxDeviationRadius / diffVal))); } // Save the data from this frame history[jointIndex].RawPosition = rawPosition; history[jointIndex].FilteredPosition = filteredPosition; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered data back into the joint // Joint j = skeleton.Joints[jt]; // j.Position = KinectHelper.Vector3ToSkeletonPoint(predictedPosition); // skeleton.Joints[jt] = j; skeleton.SkeletonPositions[jointIndex] = (Vector4)predictedPosition; }
// Update the filter for one joint. protected void FilterJoint(ref KinectCommon.NuiSkeletonData skeleton, int jointIndex, ref KinectCommon.NuiTransformSmoothParameters smoothingParameters, ref Matrix4x4[] jointOrientations) { // if (null == skeleton) // { // return; // } // int jointIndex = (int)jt; Quaternion filteredOrientation; Quaternion trend; Vector3 fwdVector = (Vector3)jointOrientations[jointIndex].GetColumn(2); if (fwdVector == Vector3.zero) { return; } Quaternion rawOrientation = Quaternion.LookRotation(fwdVector, jointOrientations[jointIndex].GetColumn(1)); Quaternion prevFilteredOrientation = this.history[jointIndex].FilteredBoneOrientation; Quaternion prevTrend = this.history[jointIndex].Trend; Vector3 rawPosition = (Vector3)skeleton.SkeletonPositions[jointIndex]; bool orientationIsValid = KinectHelper.JointPositionIsValid(rawPosition) && KinectHelper.IsTrackedOrInferred(skeleton, jointIndex) && KinectHelper.BoneOrientationIsValid(rawOrientation); if (!orientationIsValid) { if (this.history[jointIndex].FrameCount > 0) { rawOrientation = history[jointIndex].FilteredBoneOrientation; history[jointIndex].FrameCount = 0; } } // Initial start values or reset values if (this.history[jointIndex].FrameCount == 0) { // Use raw position and zero trend for first value filteredOrientation = rawOrientation; trend = Quaternion.identity; } else if (this.history[jointIndex].FrameCount == 1) { // Use average of two positions and calculate proper trend for end value Quaternion prevRawOrientation = this.history[jointIndex].RawBoneOrientation; filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f); Quaternion diffStarted = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffStarted, smoothingParameters.fCorrection); } else { // First apply a jitter filter Quaternion diffJitter = KinectHelper.RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation); float diffValJitter = (float)Math.Abs(KinectHelper.QuaternionAngle(diffJitter)); if (diffValJitter <= smoothingParameters.fJitterRadius) { filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParameters.fJitterRadius); } else { filteredOrientation = rawOrientation; } // Now the double exponential smoothing filter filteredOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, prevFilteredOrientation * prevTrend, smoothingParameters.fSmoothing); diffJitter = KinectHelper.RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation); trend = KinectHelper.EnhancedQuaternionSlerp(prevTrend, diffJitter, smoothingParameters.fCorrection); } // Use the trend and predict into the future to reduce latency Quaternion predictedOrientation = filteredOrientation * KinectHelper.EnhancedQuaternionSlerp(Quaternion.identity, trend, smoothingParameters.fPrediction); // Check that we are not too far away from raw data Quaternion diff = KinectHelper.RotationBetweenQuaternions(predictedOrientation, filteredOrientation); float diffVal = (float)Math.Abs(KinectHelper.QuaternionAngle(diff)); if (diffVal > smoothingParameters.fMaxDeviationRadius) { predictedOrientation = KinectHelper.EnhancedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParameters.fMaxDeviationRadius / diffVal); } // predictedOrientation.Normalize(); // filteredOrientation.Normalize(); // trend.Normalize(); // Save the data from this frame history[jointIndex].RawBoneOrientation = rawOrientation; history[jointIndex].FilteredBoneOrientation = filteredOrientation; history[jointIndex].Trend = trend; history[jointIndex].FrameCount++; // Set the filtered and predicted data back into the bone orientation if (KinectHelper.BoneOrientationIsValid(predictedOrientation)) { jointOrientations[jointIndex].SetTRS(Vector3.zero, predictedOrientation, Vector3.one); } }