kinect/codes/Azure-Kinect-Samples/body-tracking-samples/sample_unity_bodytracking/Assets/Skeleton/Smoother.cs

79 lines
2.6 KiB
C#
Raw Normal View History

2024-03-06 18:05:53 +00:00
using System.Collections.Generic;
public class Smoother
{
// Max allowed size of history list.
private int maxSize = 100;
// In case smoother has enough frames in history to perform smooth action.
private bool hasEnoughForSmoothing = false;
// Number of the latest frames used to smooth current position; default 5.
public int NumberSmoothingFrames { get; set; } = 5;
// Holds received data about moves.
private List<SkeletonPosition> rawData = new List<SkeletonPosition>();
// Holds received which are smoothened a little bit.
private List<SkeletonPosition> smoothenedData = new List<SkeletonPosition>();
// Process skeleton position and sends back smoothened or raw based on passed parameter.
public SkeletonPosition ReceiveNewSensorData(SkeletonPosition newData, bool smoothing)
{
// In case list is too big.
if(rawData.Count > maxSize)
{
Resize();
}
// Add new frame data to raw data used for smoothing.
rawData.Add(newData);
// In case value for smoothing is invalid just return original raw frame.
if (NumberSmoothingFrames <= 1)
{
return rawData[rawData.Count - 1];
}
// Mark that smoother has enough frames for smoothing.
if (rawData.Count > NumberSmoothingFrames)
{
hasEnoughForSmoothing = true;
}
if (smoothenedData.Count == 0)
{
smoothenedData.Add(newData);
}
else
{
SkeletonPosition temp = smoothenedData[smoothenedData.Count - 1] + newData;
if(hasEnoughForSmoothing)
{
temp = temp - rawData[rawData.Count - NumberSmoothingFrames];
}
smoothenedData.Add(temp);
}
// Smoothened timestamp has the same timestamp as the latest received.
smoothenedData[smoothenedData.Count - 1].Timestamp = rawData[rawData.Count - 1].Timestamp;
return smoothing && hasEnoughForSmoothing
? smoothenedData[smoothenedData.Count - 1] / (float)NumberSmoothingFrames
: rawData[rawData.Count - 1];
}
// Deletes old position data from list which do not have more impact on smoothing algorithm.
public void Resize()
{
if (rawData.Count > NumberSmoothingFrames)
{
rawData.RemoveRange(0, rawData.Count - NumberSmoothingFrames);
}
if (smoothenedData.Count > NumberSmoothingFrames)
{
smoothenedData.RemoveRange(0, smoothenedData.Count - NumberSmoothingFrames);
}
}
}