public Joint UpdateJoint(Kinect.JointType jt, Joint rawJoint, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
Joint prevFilteredJoint;
Joint prevRawJoint;
Joint prevTrend;
Joint filteredJoint;
Joint predictedJoint;
Joint diff;
Joint trend;
float fDiff;
prevFilteredJoint = history[jt].FilteredJoint;
prevTrend = history[jt].Trend;
prevRawJoint = history[jt].RawJoint;
// if joint is invalid, reset the filter
bool jointIsValid = JointPositionIsValid(rawJoint);
if (!jointIsValid)
{
history[jt].FrameCount = 0;
}
// initial start values
if (history[jt].FrameCount == 0)
{
filteredJoint = rawJoint;
trend = new Joint();
history[jt].FrameCount++;
}
else if (history[jt].FrameCount == 1)
{
filteredJoint = Joint.Scale(Joint.Add(rawJoint, prevRawJoint), 0.5f);
diff = Joint.Subtract(filteredJoint, prevFilteredJoint);
trend = Joint.Add(Joint.Scale(diff, smoothingParams.fCorrection), Joint.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
history[jt].FrameCount++;
}
else
{
// First apply jitter filter
diff = Joint.Subtract(rawJoint, prevFilteredJoint);
fDiff = diff.Position.magnitude;
if (fDiff <= smoothingParams.fJitterRadius)
{
filteredJoint = Joint.Add(Joint.Scale(rawJoint, fDiff / smoothingParams.fJitterRadius),
Joint.Scale(prevFilteredJoint, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
filteredJoint = rawJoint;
}
// Now the double exponential smoothing filter
filteredJoint = Joint.Add(Joint.Scale(filteredJoint, 1.0f - smoothingParams.fSmoothing),
Joint.Scale(Joint.Add(prevFilteredJoint, prevTrend), smoothingParams.fSmoothing));
diff = Joint.Subtract(filteredJoint, prevFilteredJoint);
trend = Joint.Add(Joint.Scale(diff, smoothingParams.fCorrection), Joint.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
predictedJoint = Joint.Add(filteredJoint, Joint.Scale(trend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
diff = Joint.Subtract(predictedJoint, rawJoint);
fDiff = diff.Position.magnitude;
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
predictedJoint = Joint.Add(Joint.Scale(predictedJoint, smoothingParams.fMaxDeviationRadius / fDiff),
Joint.Scale(rawJoint, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
history[jt].RawJoint = rawJoint;
history[jt].FilteredJoint = filteredJoint;
history[jt].Trend = trend;
// Output the data
filteredJoints[jt] = predictedJoint;
return(predictedJoint);
}
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
CameraSpacePoint vPrevRawPosition;
CameraSpacePoint vPrevFilteredPosition;
CameraSpacePoint vPrevTrend;
CameraSpacePoint vRawPosition;
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vPredictedPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
bool bJointIsValid;
Joint joint = body.Joints[jt];
vRawPosition = joint.Position;
vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
vPrevTrend = m_pHistory[(int)jt].m_vTrend;
vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
m_pHistory[(int)jt].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[(int)jt].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CSVectorZero();
m_pHistory[(int)jt].m_dwFrameCount++;
}
else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
{
vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_pHistory[(int)jt].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
m_pHistory[(int)jt].m_vRawPosition = vRawPosition;
m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
m_pHistory[(int)jt].m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[(int)jt] = vPredictedPosition;
}
public void JointUpdate(Dictionary <Kinect.JointType, Kinect.Joint> joints, Dictionary <Kinect.JointType, Kinect.JointOrientation> orients, int JointID, Kinect.JointType type, Kinect.TrackingState state, TRANSFORM_SMOOTH_PARAMETERS smoothingParams, int body_id = 0)
{
//printf("body: %d\n", body_id);
Vector3 vPrevRawPosition;
Vector3 vPrevFilteredPosition;
Vector3 vPrevTrend;
Vector3 vRawPosition;
Vector3 vFilteredPosition;
Vector3 vPredictedPosition;
Vector3 vDiff;
Vector3 vTrend;
float vLength;
float fDiff;
bool bJointIsValid;
Quaternion filteredOrientation;
Quaternion trend;
Quaternion rawOrientation = new Quaternion(orients[(Kinect.JointType)JointID].Orientation.X, orients[(Kinect.JointType)JointID].Orientation.Y, orients[(Kinect.JointType)JointID].Orientation.Z, orients[(Kinect.JointType)JointID].Orientation.W);
Quaternion prevFilteredOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation;
Quaternion prevTrend = m_pHistory[body_id, JointID].m_vTrendOrientation;
Kinect.Joint joint = joints[(Kinect.JointType)JointID];
Kinect.JointOrientation orient = orients[(Kinect.JointType)JointID];
vRawPosition = new Vector4(joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f);
vPrevFilteredPosition = m_pHistory[body_id, JointID].m_vFilteredPosition;
vPrevTrend = m_pHistory[body_id, JointID].m_vTrend;
vPrevRawPosition = m_pHistory[body_id, JointID].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
rawOrientation = m_pHistory[body_id, JointID].m_vFilteredOrientation;
m_pHistory[body_id, JointID].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[body_id, JointID].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = Vector3.zero;
m_pHistory[body_id, JointID].m_dwFrameCount++;
filteredOrientation = rawOrientation;
trend = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
}
else if (m_pHistory[body_id, JointID].m_dwFrameCount == 1)
{
vFilteredPosition = (vRawPosition + vPrevRawPosition) * 0.5f;
vDiff = vFilteredPosition - vPrevFilteredPosition;
vTrend = (vDiff * smoothingParams.fCorrection) + vPrevTrend * (1.0f - smoothingParams.fCorrection);
m_pHistory[body_id, JointID].m_dwFrameCount++;
Quaternion prevRawOrientation = m_pHistory[body_id, JointID].m_vRawOrientation;
filteredOrientation = EnhansedQuaternionSlerp(prevRawOrientation, rawOrientation, 0.5f);
Quaternion diffStarted = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = EnhansedQuaternionSlerp(prevTrend, diffStarted, smoothingParams.fCorrection);
}
else
{
// First apply jitter filter
vDiff = vRawPosition - vPrevFilteredPosition;
vLength = Mathf.Sqrt(vDiff.sqrMagnitude);
fDiff = Mathf.Abs(vLength);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = vRawPosition * (fDiff / smoothingParams.fJitterRadius) + vPrevFilteredPosition * (1.0f - fDiff / smoothingParams.fJitterRadius);
}
else
{
vFilteredPosition = vRawPosition;
}
Quaternion diffJitter = RotationBetweenQuaternions(rawOrientation, prevFilteredOrientation);
float diffValJitter = Mathf.Abs(QuaternionAngle(diffJitter));
if (diffValJitter <= smoothingParams.fJitterRadius)
{
filteredOrientation = EnhansedQuaternionSlerp(prevFilteredOrientation, rawOrientation, diffValJitter / smoothingParams.fJitterRadius);
}
else
{
filteredOrientation = rawOrientation;
}
// Now the double exponential smoothing filter
vFilteredPosition = vFilteredPosition * (1.0f - smoothingParams.fSmoothing) + (vPrevFilteredPosition + vPrevTrend) * smoothingParams.fSmoothing;
vDiff = vFilteredPosition - vPrevFilteredPosition;
vTrend = ((vDiff * smoothingParams.fCorrection) + (vPrevTrend * (1.0f - smoothingParams.fCorrection)));
// Now the double exponential smoothing filter
filteredOrientation = EnhansedQuaternionSlerp(filteredOrientation, (prevFilteredOrientation * prevTrend), smoothingParams.fSmoothing);
diffJitter = RotationBetweenQuaternions(filteredOrientation, prevFilteredOrientation);
trend = EnhansedQuaternionSlerp(prevTrend, diffJitter, smoothingParams.fCorrection);
}
// Predict into the future to reduce latency
vPredictedPosition = (vFilteredPosition + (vTrend * smoothingParams.fPrediction));
Quaternion predictedOrientation = (filteredOrientation * (EnhansedQuaternionSlerp(new Quaternion(0.0f, 0.0f, 0.0f, 1.0f), trend, smoothingParams.fPrediction)));
// Check that we are not too far away from raw data
vDiff = (vRawPosition - vPrevFilteredPosition);
vLength = Mathf.Sqrt(vDiff.sqrMagnitude);
fDiff = Mathf.Abs(vLength);
Quaternion diff = RotationBetweenQuaternions(predictedOrientation, filteredOrientation);
float diffVal = Mathf.Abs(QuaternionAngle(diff));
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = ((vPredictedPosition * (smoothingParams.fMaxDeviationRadius / fDiff)) + (vRawPosition * (1.0f - smoothingParams.fMaxDeviationRadius / fDiff)));
}
if (diffVal > smoothingParams.fMaxDeviationRadius)
{
predictedOrientation = EnhansedQuaternionSlerp(filteredOrientation, predictedOrientation, smoothingParams.fMaxDeviationRadius / diffVal);
}
predictedOrientation = QuaternionNormalise(predictedOrientation);
filteredOrientation = QuaternionNormalise(filteredOrientation);
trend = QuaternionNormalise(trend);
// Save the data from this frame
m_pHistory[body_id, JointID].m_vRawPosition = vRawPosition;
m_pHistory[body_id, JointID].m_vFilteredPosition = vFilteredPosition;
m_pHistory[body_id, JointID].m_vTrend = vTrend;
if (!(rawOrientation == null) && !(filteredOrientation == null) && !(trend == null))
{
m_pHistory[body_id, JointID].m_vRawOrientation = rawOrientation;
m_pHistory[body_id, JointID].m_vFilteredOrientation = filteredOrientation;
m_pHistory[body_id, JointID].m_vTrendOrientation = trend;
}
// Output the data
//vPredictedPosition = new Vector4(vPredictedPosition , 1.0f);
Kinect.Joint j = new Windows.Kinect.Joint();
Kinect.CameraSpacePoint CameraSpacePoint = new Windows.Kinect.CameraSpacePoint();
CameraSpacePoint.X = vPredictedPosition.x;
CameraSpacePoint.Y = vPredictedPosition.y;
CameraSpacePoint.Z = vPredictedPosition.z;
j.Position = CameraSpacePoint;
j.JointType = type;
j.TrackingState = state;
m_pFilteredJoints[(Kinect.JointType)JointID] = j;
// HipCenter has no parent and is the root of our skeleton - leave the HipCenter absolute set as it is
if (type != Kinect.JointType.SpineBase)
{
Kinect.JointOrientation jo = new Windows.Kinect.JointOrientation();
Kinect.Vector4 v4 = new Kinect.Vector4();
v4.X = predictedOrientation.x;
v4.Y = predictedOrientation.y;
v4.Z = predictedOrientation.z;
v4.W = predictedOrientation.w;
jo.Orientation = v4;
jo.JointType = type;
m_pFilteredOrientations[(Kinect.JointType)JointID] = jo;
}
else
{
m_pFilteredOrientations[(Kinect.JointType)JointID] = orients[(Kinect.JointType)JointID];
}
//m_pFilteredJoints[JointID]
}
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
CameraSpacePoint vPrevRawPosition;
CameraSpacePoint vPrevFilteredPosition;
CameraSpacePoint vPrevTrend;
CameraSpacePoint vRawPosition;
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vPredictedPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
bool bJointIsValid;
Joint joint = body.Joints[jt];
vRawPosition = joint.Position;
vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
vPrevTrend = m_pHistory[(int)jt].m_vTrend;
vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
m_pHistory[(int)jt].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[(int)jt].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CSVectorZero();
m_pHistory[(int)jt].m_dwFrameCount++;
}
else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
{
vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_pHistory[(int)jt].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
m_pHistory[(int)jt].m_vRawPosition = vRawPosition;
m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
m_pHistory[(int)jt].m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[(int)jt] = vPredictedPosition;
}
private void Update(FilteredTransform rawJoint, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
FilteredTransform prevFilteredJoint;
FilteredTransform prevRawJoint;
FilteredTransform prevTrend;
FilteredTransform predictedJoint;
FilteredTransform diff;
FilteredTransform trend;
float fDiff;
prevFilteredJoint = history.FilteredJoint;
prevTrend = history.Trend;
prevRawJoint = history.RawJoint;
// initial start values
if (history.FrameCount == 0)
{
filteredTransform = rawJoint;
trend = new FilteredTransform();
history.FrameCount++;
}
else if (history.FrameCount == 1)
{
filteredTransform = FilteredTransform.Scale(FilteredTransform.Add(rawJoint, prevRawJoint), 0.5f);
diff = FilteredTransform.Subtract(filteredTransform, prevFilteredJoint);
trend = FilteredTransform.Add(FilteredTransform.Scale(diff, smoothingParams.fCorrection), FilteredTransform.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
history.FrameCount++;
}
else
{
// First apply jitter filter
diff = FilteredTransform.Subtract(rawJoint, prevFilteredJoint);
fDiff = diff.Position.magnitude;
if (fDiff <= smoothingParams.fJitterRadius)
{
filteredTransform = FilteredTransform.Add(FilteredTransform.Scale(rawJoint, fDiff / smoothingParams.fJitterRadius),
FilteredTransform.Scale(prevFilteredJoint, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
filteredTransform = rawJoint;
}
// Now the double exponential smoothing filter
filteredTransform = FilteredTransform.Add(FilteredTransform.Scale(filteredTransform, 1.0f - smoothingParams.fSmoothing),
FilteredTransform.Scale(FilteredTransform.Add(prevFilteredJoint, prevTrend), smoothingParams.fSmoothing));
diff = FilteredTransform.Subtract(filteredTransform, prevFilteredJoint);
trend = FilteredTransform.Add(FilteredTransform.Scale(diff, smoothingParams.fCorrection), FilteredTransform.Scale(prevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
predictedJoint = FilteredTransform.Add(filteredTransform, FilteredTransform.Scale(trend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
diff = FilteredTransform.Subtract(predictedJoint, rawJoint);
fDiff = diff.Position.magnitude;
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
predictedJoint = FilteredTransform.Add(FilteredTransform.Scale(predictedJoint, smoothingParams.fMaxDeviationRadius / fDiff),
FilteredTransform.Scale(rawJoint, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
history.RawJoint = rawJoint;
history.FilteredJoint = filteredTransform;
history.Trend = trend;
// Output the data
filteredTransform = predictedJoint;
}
