private void Skele_OnFrame(KinCapFrame OneFrame)
{
if (frameTotCnt < MaxFrames)
{
if (OneFrame.BodyNdx == 0) // we're only grabbing body 0
// copy frame data including joints, positions, tracking, etc
{
capFrames[frameTotCnt].BodyNdx = OneFrame.BodyNdx;
capFrames[frameTotCnt].FloorClip = OneFrame.FloorClip;
capFrames[frameTotCnt].TmSpan = OneFrame.TmSpan;
capFrames[frameTotCnt].FrameNum = OneFrame.FrameNum;
for (int z = 0; z <= OneFrame.CamPos.GetUpperBound(0); z++)
{
capFrames[frameTotCnt].CamPos[z] = OneFrame.CamPos[z];
capFrames[frameTotCnt].SmoothedCSP[z] = OneFrame.SmoothedCSP[z];
capFrames[frameTotCnt].TrackState[z] = OneFrame.TrackState[z];
}
if (frameTotCnt % 10 == 0)
{
Frame_Lbl.Text = string.Format("Frames: {0}", frameTotCnt);
}
frameTotCnt++;
}
}
else
{
Frame_Lbl.Text = "Max Frames hit!";
Frame_Lbl.Refresh();
}
}
/// <summary>Update the filter with a new frame of data and smooth.</summary>
public void UpdateFilter(ref KinCapFrame CapFrame)
{
Array jointTypeValues = Enum.GetValues(typeof(JointType));
TransSmoothParms tempSmoothingParams = new TransSmoothParms();
smoothParameters.JitterRadius = Math.Max(0.0001f, smoothParameters.JitterRadius);
tempSmoothingParams.Smoothing = smoothParameters.Smoothing;
tempSmoothingParams.Correction = smoothParameters.Correction;
tempSmoothingParams.Prediction = smoothParameters.Prediction;
foreach (JointType jt in jointTypeValues)
{
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
int jNdx = (int)jt;
if ((CapFrame.TrackState[jNdx] == TrackingState.Tracked) && (IsFeet(jt) == false))
{
tempSmoothingParams.JitterRadius = smoothParameters.JitterRadius;
tempSmoothingParams.MaxDeviationRadius = smoothParameters.MaxDeviationRadius;
}
else
{
tempSmoothingParams.JitterRadius *= 2.0f;
tempSmoothingParams.MaxDeviationRadius *= 2.0f;
}
FilterJoint(ref CapFrame, jNdx, tempSmoothingParams);
}
}
private void InitCapFrames()
{
// init buffers for frames
for (int z = 0; z < MaxFrames; z++)
{
capFrames[z] = new KinCapFrame();
}
}
/// <summary>Update the filter for one joint.</summary>
/// <param name="skeleton">The Skeleton to filter.</param>
/// <param name="jt">The Skeleton Joint index to filter.</param>
/// <param name="smoothingParameters">The Smoothing parameters to apply.</param>
protected void FilterJoint(ref KinCapFrame capFrame, int jt, TransSmoothParms smoothingParameters)
{
Vector3 filteredPosition;
Vector3 diffvec;
Vector3 trend;
float diffVal;
Vector3 rawPosition = new Vector3(capFrame.SmoothedCSP[jt].X, capFrame.SmoothedCSP[jt].Y, capFrame.SmoothedCSP[jt].Z);
Vector3 prevFilteredPosition = history[jt].FilteredPosition;
Vector3 prevTrend = history[jt].Trend;
Vector3 prevRawPosition = history[jt].RawPosition;
// If joint is invalid, reset the filter
if ((capFrame.SmoothedCSP[jt].X == 0.0f) && (capFrame.SmoothedCSP[jt].Y == 0.0f) & (capFrame.SmoothedCSP[jt].Z == 0.0f))
{
history[jt].FrameCount = 0;
}
// Initial start values
if (history[jt].FrameCount == 0)
{
filteredPosition = rawPosition;
trend = Vector3.Zero;
}
else if (history[jt].FrameCount == 1)
{
filteredPosition = Vector3.Multiply(Vector3.Add(rawPosition, prevRawPosition), 0.5f);
diffvec = Vector3.Subtract(filteredPosition, prevFilteredPosition);
trend = Vector3.Add(Vector3.Multiply(diffvec, smoothingParameters.Correction), Vector3.Multiply(prevTrend, 1.0f - smoothingParameters.Correction));
}
else
{
// First apply jitter filter
diffvec = Vector3.Subtract(rawPosition, prevFilteredPosition);
diffVal = Math.Abs(diffvec.Length());
if (diffVal <= smoothingParameters.JitterRadius)
{
filteredPosition = Vector3.Add(Vector3.Multiply(rawPosition, diffVal / smoothingParameters.JitterRadius), Vector3.Multiply(prevFilteredPosition, 1.0f - (diffVal / smoothingParameters.JitterRadius)));
}
else
{
filteredPosition = rawPosition;
}
// Now the double exponential smoothing filter
filteredPosition = Vector3.Add(Vector3.Multiply(filteredPosition, 1.0f - smoothingParameters.Smoothing), Vector3.Multiply(Vector3.Add(prevFilteredPosition, prevTrend), smoothingParameters.Smoothing));
diffvec = Vector3.Subtract(filteredPosition, prevFilteredPosition);
trend = Vector3.Add(Vector3.Multiply(diffvec, smoothingParameters.Correction), Vector3.Multiply(prevTrend, 1.0f - smoothingParameters.Correction));
}
// Predict into the future to reduce latency
Vector3 predictedPosition = Vector3.Add(filteredPosition, Vector3.Multiply(trend, smoothingParameters.Prediction));
// Check that we are not too far away from raw data
diffvec = Vector3.Subtract(predictedPosition, rawPosition);
diffVal = Math.Abs(diffvec.Length());
if (diffVal > smoothingParameters.MaxDeviationRadius)
{
predictedPosition = Vector3.Add(Vector3.Multiply(predictedPosition, smoothingParameters.MaxDeviationRadius / diffVal), Vector3.Multiply(rawPosition, 1.0f - (smoothingParameters.MaxDeviationRadius / diffVal)));
}
// Save the data from this frame
history[jt].RawPosition = rawPosition;
history[jt].FilteredPosition = filteredPosition;
history[jt].Trend = trend;
history[jt].FrameCount++;
// Set the filtered data back into the joint
capFrame.SmoothedCSP[jt].X = predictedPosition.X;
capFrame.SmoothedCSP[jt].Y = predictedPosition.Y;
capFrame.SmoothedCSP[jt].Z = predictedPosition.Z;
}
public KinSkeleTrack()
{
Frm = new KinCapFrame();
}