// 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);
}
}
