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.sqrMagnitude;// CSVectorLength(diff); 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.sqrMagnitude;// XMVector3Length(vLength); 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); }
private static void CreateJoint(Dictionary <string, Joint> list, Transform bone, Joint parent) { // check that it is not already created Joint node = list.FirstOrDefault(x => x.Key == bone.name).Value; if (node != null) { return; } // create the joint and add to the list node = new Joint(); node.Init(bone.name); node.SetRawtData(bone.position, bone.rotation); list.Add(bone.name, node); // if this is a child joint, set the property if (parent != null) { parent.AddChild(node); } // traverse children to create tree foreach (Transform child in bone) { CreateJoint(list, child, node); } }