public void Reset(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
m_fSmoothing = fSmoothing; // How much smothing will occur. Will lag when too high
m_fCorrection = fCorrection; // How much to correct back from prediction. Can make things springy
m_fPrediction = fPrediction; // Amount of prediction into the future to use. Can over shoot when too high
m_fJitterRadius = fJitterRadius; // Size of the radius where jitter is removed. Can do too much smoothing when too high
m_pFilteredJoints = new Dictionary <Windows.Kinect.JointType, Windows.Kinect.Joint>();
m_pFilteredOrientations = new Dictionary <Windows.Kinect.JointType, Windows.Kinect.JointOrientation>();
m_pHistory = new FilterDoubleExponentialData[BODY_COUNT, JointType_Count];
for (int i = 0; i < BODY_COUNT; i++)
{
for (int j = 0; j < JointType_Count; j++)
{
m_pHistory[i, j] = new FilterDoubleExponentialData();
}
}
/*
* memset(m_pFilteredJoints, 0, sizeof(Joint) * JointType_Count);
* memset(m_pFilteredOrientations, 0, sizeof(JointOrientation) * JointType_Count);
* memset(m_pHistory[0], 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
* memset(m_pHistory[1], 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
* memset(m_pHistory[2], 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
* memset(m_pHistory[3], 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
* memset(m_pHistory[4], 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
* memset(m_pHistory[5], 0, sizeof(FilterDoubleExponentialData) * JointType_Count);
*/
}
public KinectJointFilter(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++) {
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
public KinectJointFilter()
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init();
}
public KinectJointFilter()
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init();
}
public KinectJointOrientationFilter()
{
m_pFilteredJoints = new Windows.Kinect.Vector4[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init();
}
public KinectJointFilter(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
/// <summary>
/// Create an exponential smoothing joint with default configuration values.
/// </summary>
/// <param name="jointType">The joint type to create</param>
public ExponentialJoint(JointType jointType)
: base(jointType)
{
var parms = new ExponentialSmoothingParameters();
_smoothing = parms.Smoothing;
_correction = parms.Correction;
_prediction = parms.Prediction;
_jitterRadius = parms.JitterRadius;
_maxDeviationRadius = parms.MaxDeviationRadius;
_history = new FilterDoubleExponentialData();
}
public KinectJointFilter()
{
m_pFilteredJoints = new CameraSpacePoint[Body.JointCount];
filteredJointPositionsMap = new Dictionary <JointType, Vector3>();
m_pHistory = new FilterDoubleExponentialData[Body.JointCount];
for (int i = 0; i < Body.JointCount; i++)
{
m_pHistory[i] = new FilterDoubleExponentialData();
}
Init();
}
/// <summary>
/// Create an exponential smoothing joint with default configuration values.
/// </summary>
/// <param name="jointType">The joint type to create</param>
public ExponentialJoint(JointType jointType)
: base(jointType)
{
var parms = new ExponentialSmoothingParameters();
_smoothing = parms.Smoothing;
_correction = parms.Correction;
_prediction = parms.Prediction;
_jitterRadius = parms.JitterRadius;
_maxDeviationRadius = parms.MaxDeviationRadius;
_history = new FilterDoubleExponentialData();
}
public void Init(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
{
m_pFilteredJoints = new Dictionary <Windows.Kinect.JointType, Kinect.Joint>(JointType_Count);
m_pFilteredOrientations = new Dictionary <Windows.Kinect.JointType, Windows.Kinect.JointOrientation>(JointType_Count);
m_pHistory = new FilterDoubleExponentialData[BODY_COUNT, JointType_Count];
for (int i = 0; i < BODY_COUNT; i++)
{
for (int j = 0; j < JointType_Count; j++)
{
m_pHistory[i, j] = new FilterDoubleExponentialData();
}
}
Reset(fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius);
}
/// <summary>
/// Create an exponential smoothing joint with custom configuration values.
/// </summary>
/// <param name="jointType">The joint type to create</param>
/// <param name="parameters">An <c>ExponentialSmoothingParameters</c> object</param>
public ExponentialJoint(JointType jointType, ISmootherParameters parameters = null)
: base(jointType)
{
var parms = parameters as ExponentialSmoothingParameters;
if (parms == null)
parms = new ExponentialSmoothingParameters();
_smoothing = parms.Smoothing;
_correction = parms.Correction;
_prediction = parms.Prediction;
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
_jitterRadius = Math.Max(0.0001f, parms.JitterRadius);
_maxDeviationRadius = parms.MaxDeviationRadius;
_history = new FilterDoubleExponentialData();
}
/// <summary>
/// Create an exponential smoothing joint with custom configuration values.
/// </summary>
/// <param name="jointType">The joint type to create</param>
/// <param name="parameters">An <c>ExponentialSmoothingParameters</c> object</param>
public ExponentialJoint(JointType jointType, ISmootherParameters parameters = null)
: base(jointType)
{
var parms = parameters as ExponentialSmoothingParameters;
if (parms == null)
{
parms = new ExponentialSmoothingParameters();
}
_smoothing = parms.Smoothing;
_correction = parms.Correction;
_prediction = parms.Prediction;
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
_jitterRadius = Math.Max(0.0001f, parms.JitterRadius);
_maxDeviationRadius = parms.MaxDeviationRadius;
_history = new FilterDoubleExponentialData();
}
private void Reset()
{
_history = new FilterDoubleExponentialData();
}
private void Reset()
{
_history = new FilterDoubleExponentialData();
}
private void Update(Joint joint, TransformSmoothParameters smoothingParams)
{
FilterDoubleExponentialData history = m_pHistory[joint.JointType];
Vector3 vPrevRawPosition = history.m_vRawPosition;
Vector3 vPrevFilteredPosition = history.m_vFilteredPosition;
Vector3 vPrevTrend = history.m_vTrend;
Vector3 vRawPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
Vector3 vFilteredPosition;
Vector3 vPredictedPosition;
Vector3 vDiff;
Vector3 vTrend;
//Vector3 vLength;
float fDiff;
bool bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
history.m_dwFrameCount = 0;
}
// Initial start values
if (history.m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = Vector3.Zero;
history.m_dwFrameCount++;
}
else if (history.m_dwFrameCount == 1)
{
vFilteredPosition = (vRawPosition + vPrevRawPosition) * 0.5f; // XMVectorScale(XMVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = vFilteredPosition - vPrevFilteredPosition; // XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = (vDiff * smoothingParams.fCorrection) + (vPrevTrend * (1.0f - smoothingParams.fCorrection)); // XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
history.m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = vRawPosition - vPrevFilteredPosition; // XMVectorSubtract(vRawPosition, vPrevFilteredPosition);
//vLength = XMVector3Length(vDiff);
fDiff = Math.Abs(vDiff.length()); // fabs(XMVectorGetX(vLength));
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = (vRawPosition * (fDiff / smoothingParams.fJitterRadius)) + (vPrevFilteredPosition * (1.0f - fDiff / smoothingParams.fJitterRadius));
//XMVectorAdd(XMVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius), XMVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = (vFilteredPosition * (1.0f - smoothingParams.fSmoothing)) + ((vPrevFilteredPosition + vPrevTrend) * smoothingParams.fSmoothing);
//XMVectorAdd(XMVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing), XMVectorScale(XMVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = vFilteredPosition - vPrevFilteredPosition;// XMVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = (vDiff * smoothingParams.fCorrection) + (vPrevTrend * (1.0f - smoothingParams.fCorrection));
// XMVectorAdd(XMVectorScale(vDiff, smoothingParams.fCorrection), XMVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = vFilteredPosition + (vTrend * smoothingParams.fPrediction);// XMVectorAdd(vFilteredPosition, XMVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = vPredictedPosition - vRawPosition; // XMVectorSubtract(vPredictedPosition, vRawPosition);
//vLength = XMVector3Length(vDiff);
fDiff = Math.Abs(vDiff.length()); // fabs(XMVectorGetX(vLength));
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = (vPredictedPosition * (smoothingParams.fMaxDeviationRadius / fDiff)) + (vRawPosition * (1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
//XMVectorAdd(XMVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff), XMVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
history.m_vRawPosition = vRawPosition;
history.m_vFilteredPosition = vFilteredPosition;
history.m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[joint.JointType] = vPredictedPosition;
//m_pFilteredJoints[JointID] = vPredictedPosition;
//m_pFilteredJoints[JointID] = XMVectorSetW(m_pFilteredJoints[JointID], 1.0f);
}