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