// Initialize the filter with a set of SmoothParameters.
public void Init(SmoothParameters smoothParameters)
{
this.smoothingType = SmoothingType.Default;
this.smoothParameters = smoothParameters;
Reset();
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.smoothingType = SmoothingType.Default;
smoothParameters = new SmoothParameters();
smoothParameters.smoothing = smoothingValue; // How much soothing will occur. Will lag when too high
smoothParameters.correction = correctionValue; // How much to correct back from prediction. Can make things springy
smoothParameters.prediction = predictionValue; // Amount of prediction into the future to use. Can over shoot when too high
smoothParameters.jitterRadius = jitterRadiusValue; // Size of the radius where jitter is removed. Can do too much smoothing when too high
smoothParameters.maxDeviationRadius = maxDeviationRadiusValue; // Size of the max prediction radius Can snap back to noisy data when too high
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
smoothParameters.jitterRadius = Math.Max(0.0001f, this.smoothParameters.jitterRadius);
Reset();
init = true;
}
// Initialize the filter with a set of SmoothParameters.
public void Init(SmoothingType smoothingType)
{
this.smoothingType = smoothingType;
smoothParameters = new SmoothParameters();
switch (smoothingType)
{
case SmoothingType.Light:
smoothParameters.smoothing = 0.3f;
smoothParameters.correction = 0.35f;
smoothParameters.prediction = 0.35f;
smoothParameters.jitterRadius = 0.15f;
smoothParameters.maxDeviationRadius = 0.15f;
break;
case SmoothingType.Medium:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.1f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.1f;
smoothParameters.maxDeviationRadius = 0.1f;
break;
case SmoothingType.Aggressive:
smoothParameters.smoothing = 0.7f;
smoothParameters.correction = 0.3f;
smoothParameters.prediction = 1.0f;
smoothParameters.jitterRadius = 1.0f;
smoothParameters.maxDeviationRadius = 1.0f;
break;
//case SmoothingType.Default:
default:
smoothParameters.smoothing = 0.5f;
smoothParameters.correction = 0.5f;
smoothParameters.prediction = 0.5f;
smoothParameters.jitterRadius = 0.05f;
smoothParameters.maxDeviationRadius = 0.04f;
break;
}
Reset();
init = true;
}
//// Update the filter with a new frame of data and smooth.
//public void UpdateFilter(ref KinectInterop.BodyData[] alTrackedBodies)
//{
// if (!init)
// {
// // initialize with by-default parameters
// Init();
// }
// if (smoothingType == SmoothingType.None)
// return;
// SmoothParameters tempSmoothingParams = new SmoothParameters();
// tempSmoothingParams.smoothing = this.smoothParameters.smoothing;
// tempSmoothingParams.correction = this.smoothParameters.correction;
// tempSmoothingParams.prediction = this.smoothParameters.prediction;
// int bodyCount = alTrackedBodies != null ? alTrackedBodies.Length : 0;
// for (int bodyIndex = 0; bodyIndex < bodyCount; bodyIndex++)
// {
// if (alTrackedBodies[bodyIndex].bIsTracked)
// {
// FilterBodyJoints(ref alTrackedBodies[bodyIndex], /**bodyIndex*/ alTrackedBodies[bodyIndex].iBodyIndex, tempSmoothingParams);
// }
// }
//}
// Update the filter with a new frame of data and smooth.
public void UpdateFilter(ref KinectInterop.BodyData bodyData)
{
if (!init)
{
// initialize with by-default parameters
Init();
}
if (smoothingType == SmoothingType.None)
{
return;
}
SmoothParameters tempSmoothingParams = new SmoothParameters();
tempSmoothingParams.smoothing = smoothParameters.smoothing;
tempSmoothingParams.correction = smoothParameters.correction;
tempSmoothingParams.prediction = smoothParameters.prediction;
if (bodyData.bIsTracked)
{
// get body index
int bodyIndex = GetUserIndex(bodyData.liTrackingID);
if (bodyIndex < 0)
{
bodyIndex = GetFreeIndex();
if (bodyIndex >= 0)
{
history[bodyIndex].userId = bodyData.liTrackingID;
}
//Debug.Log("Created history for userId: " + history[bodyIndex].userId + ", index: " + bodyIndex + ", time: " + DateTime.UtcNow);
}
// filter
if (bodyIndex >= 0)
{
FilterBodyJoints(ref bodyData, bodyIndex, tempSmoothingParams);
}
}
// free unused history
//CleanUpUserHistory();
}
// Update the filter for one joint
private Vector3 FilterJoint(Vector3 rawPosition, int bodyIndex, int jointIndex, SmoothParameters smoothingParameters)
{
Vector3 filteredPosition;
Vector3 diffVec;
Vector3 trend;
float diffVal;
Vector3 prevFilteredPosition = history[bodyIndex].jointHistory[jointIndex].filteredPosition;
Vector3 prevTrend = history[bodyIndex].jointHistory[jointIndex].trend;
Vector3 prevRawPosition = history[bodyIndex].jointHistory[jointIndex].rawPosition;
bool jointIsValid = (rawPosition != Vector3.zero);
// If joint is invalid, reset the filter
if (!jointIsValid)
{
history[bodyIndex].jointHistory[jointIndex].frameCount = 0;
}
// Initial start values
if (history[bodyIndex].jointHistory[jointIndex].frameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.zero;
}
else if (history[bodyIndex].jointHistory[jointIndex].frameCount == 1)
{
filteredPosition = (rawPosition + prevRawPosition) * 0.5f;
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
else
{
// First apply jitter filter
diffVec = rawPosition - prevFilteredPosition;
diffVal = Math.Abs(diffVec.magnitude);
if (diffVal <= smoothingParameters.jitterRadius)
{
filteredPosition = (rawPosition * (diffVal / smoothingParameters.jitterRadius)) + (prevFilteredPosition * (1.0f - (diffVal / smoothingParameters.jitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Now the double exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - smoothingParameters.smoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.smoothing);
diffVec = filteredPosition - prevFilteredPosition;
trend = (diffVec * smoothingParameters.correction) + (prevTrend * (1.0f - smoothingParameters.correction));
}
// Predict into the future to reduce latency
Vector3 predictedPosition = filteredPosition + (trend * smoothingParameters.prediction);
// Check that we are not too far away from raw data
diffVec = predictedPosition - rawPosition;
diffVal = Mathf.Abs(diffVec.magnitude);
if (diffVal > smoothingParameters.maxDeviationRadius)
{
predictedPosition = (predictedPosition * (smoothingParameters.maxDeviationRadius / diffVal)) + (rawPosition * (1.0f - (smoothingParameters.maxDeviationRadius / diffVal)));
}
// Save the data from this frame
history[bodyIndex].jointHistory[jointIndex].rawPosition = rawPosition;
history[bodyIndex].jointHistory[jointIndex].filteredPosition = filteredPosition;
history[bodyIndex].jointHistory[jointIndex].trend = trend;
history[bodyIndex].jointHistory[jointIndex].frameCount++;
DateTime dtNow = DateTime.UtcNow;
history[bodyIndex].lastUpdateTime = dtNow.Ticks;
//Debug.Log("Updated history for userId: " + history[bodyIndex].userId + ", index: " + bodyIndex + ", time: " + dtNow + " (" + history[bodyIndex].lastUpdateTime + ")");
return(predictedPosition);
}
// Update the filter for all body joints
private void FilterBodyJoints(ref KinectInterop.BodyData bodyData, int bodyIndex, SmoothParameters tempSmoothingParams)
{
KinectManager manager = KinectManager.Instance;
int jointCount = manager.GetJointCount();
for (int jointIndex = 0; jointIndex < jointCount; jointIndex++)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter end joints highly as they are more noisy
if (bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.Tracked ||
jointIndex == (int)KinectInterop.JointType.FootLeft || jointIndex == (int)KinectInterop.JointType.FootRight ||
jointIndex == (int)KinectInterop.JointType.HandLeft || jointIndex == (int)KinectInterop.JointType.HandRight ||
jointIndex == (int)KinectInterop.JointType.HandtipLeft || jointIndex == (int)KinectInterop.JointType.HandtipRight ||
jointIndex == (int)KinectInterop.JointType.ThumbLeft || jointIndex == (int)KinectInterop.JointType.ThumbRight)
//|| jointIndex == (int)KinectInterop.JointType.Head)
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius * 2.0f;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius * 2.0f;
}
else
{
tempSmoothingParams.jitterRadius = smoothParameters.jitterRadius;
tempSmoothingParams.maxDeviationRadius = smoothParameters.maxDeviationRadius;
}
Vector3 jPosition = bodyData.joint[jointIndex].trackingState != KinectInterop.TrackingState.NotTracked ? bodyData.joint[jointIndex].position : Vector3.zero;
bodyData.joint[jointIndex].position = FilterJoint(jPosition, bodyIndex, jointIndex, tempSmoothingParams);
}
bodyData.position = bodyData.joint[0].position;
}
