private TrackingState GetSkeletonTrackingState(KinectSkeleton skeleton) { bool anyTracked = false; bool anyInferred = false; TrackingState state = TrackingState.NotTracked; for (int i = 0; i < skeleton.skeleton.Count; i++) { if (skeleton.skeleton[i].TrackingState == TrackingState.Tracked) { anyTracked = true; state = TrackingState.Tracked; break; } else if (skeleton.skeleton[i].TrackingState == TrackingState.Inferred) { anyInferred = true; } } if (!anyTracked && anyInferred) { state = TrackingState.Inferred; } return(state); }
private void IntegrateRightHandGrabState(KinectSkeleton skeleton) { if (skeleton.skeleton[JointType.HandRight].TrackingState == TrackingState.Tracked) { rhFilter.IntegrateMeasurement(skeleton.rightHandClosed); } }
internal void IntegrateSkeleton(KinectSkeleton skeleton) { for (int i = 0; i < KinectBase.HelperMethods.TotalJointCount; i++) { if (skeleton.skeleton[i].TrackingState == TrackingState.Tracked) { filteredJoints[i].IntegrateMeasurement(PointToObMatrix(skeleton.skeleton[i].Position), skeleton.skeleton[i].utcTime, skeleton.skeleton[i].spatialErrorStdDev); if (skeleton.skeleton[i].utcTime > lastTrackedTime[i]) { lastTrackedTime[i] = skeleton.skeleton[i].utcTime; } } else if (skeleton.skeleton[i].TrackingState == TrackingState.Inferred) { filteredJoints[i].IntegrateMeasurement(PointToObMatrix(skeleton.skeleton[i].Position), skeleton.skeleton[i].utcTime, skeleton.skeleton[i].spatialErrorStdDev); if (skeleton.skeleton[i].utcTime > lastInferredTime[i]) { lastInferredTime[i] = skeleton.skeleton[i].utcTime; } } } //Determine the state of the right hand grasp if (skeleton.skeleton[JointType.HandRight].TrackingState == TrackingState.Tracked) { IntegrateLeftHandGrabState(skeleton); } //Determine the state of the left hand grasp if (skeleton.skeleton[JointType.HandLeft].TrackingState == TrackingState.Tracked) { IntegrateRightHandGrabState(skeleton); } }
internal KinectSkeleton PredictSkeleton(double msAheadOfNow) { KinectSkeleton newSkeleton = new KinectSkeleton(); Joint[] tempJoints = new Joint[KinectBase.HelperMethods.TotalJointCount]; for (int i = 0; i < KinectBase.HelperMethods.TotalJointCount; i++) { Joint newJoint = new Joint(); EigenWrapper.Matrix covariance; newJoint.JointType = newSkeleton.skeleton[i].JointType; EigenWrapper.Matrix state = filteredJoints[i].PredictAndDiscardFromNow(msAheadOfNow, out covariance); newJoint.Position = FilteredMatrixToPoint(state); newJoint.TrackingState = GetJointTrackingState(covariance, lastTrackedTime[i]); tempJoints[i] = newJoint; } //Calculate the orientations for all the skeletons Quaternion[] orientations = CalculateOrientations(tempJoints); //Add the orientations to the joints and pass them into the newSkeleton object for (int i = 0; i < KinectBase.HelperMethods.TotalJointCount; i++) { tempJoints[i].Orientation = orientations[i]; newSkeleton.skeleton[i] = tempJoints[i]; } //TODO: Handle all the per skeleton stuff here (e.g. skeleton tracking state, tracking ID, etc) newSkeleton.leftHandClosed = lhFilter.PredictMeasurement(); newSkeleton.rightHandClosed = rhFilter.PredictMeasurement(); newSkeleton.Position = newSkeleton.skeleton[JointType.HipCenter].Position; newSkeleton.SkeletonTrackingState = GetSkeletonTrackingState(newSkeleton); return(newSkeleton); }
private void parent_MergedSkeletonChanged(object sender, SkeletonEventArgs e) { if (e.skeletons.Length > 0) { if (recording) { KinectSkeleton skelCopy = HelperMethods.DeepCopySkeleton(e.skeletons[0]); trainingData.Add(skelCopy); } else if (testing) { KinectSkeleton skelCopy = HelperMethods.DeepCopySkeleton(e.skeletons[0]); double val = parent.server.gestRecog.TestRecognizer(skelCopy, index); System.Diagnostics.Trace.WriteLine("Relative Probability: " + val.ToString()); if (val < 1.0) { this.Dispatcher.BeginInvoke((Action)(() => { //probTextBlock.Text = val.ToString("F5"); probTextBlock.Text = "Found"; probTextBlock.Background = Brushes.OrangeRed; ToggleBackDelegate deli = toggleBackFound; deli.BeginInvoke(null, null); }), null); } } } }
internal double TestRecognizer(KinectSkeleton latestSkeleton, int testNumber) { if (testNumber >= 0 && testNumber < recognizers.Count) { recognizers[testNumber].AddDataPoint(latestSkeleton, masterSettings.gestureCommands[testNumber].monitoredJoint); return(recognizers[testNumber].TestGesture(masterSettings.gestureCommands[testNumber].sensitivity)); } else { return(double.PositiveInfinity); } }
internal void MergeSkeleton(KinectSkeleton skeleton) { //Only merge skeletons that have some sort of useful information if (skeleton.SkeletonTrackingState == TrackingState.Tracked || skeleton.SkeletonTrackingState == TrackingState.Inferred) { filteredSkeletons.HoldUpdates(); int filteredSkeletonIndex = FindSkeletonNumber(skeleton); IntegrateSkeleton(skeleton, filteredSkeletonIndex); filteredSkeletons.ReleaseForUpdates(); } }
private void IntegrateSkeleton(KinectSkeleton skeleton, int number) { if (number >= 0) { filteredSkeletons[number].IntegrateSkeleton(skeleton); } else { FilteredSkeleton tempSkel = new FilteredSkeleton(); tempSkel.IntegrateSkeleton(skeleton); filteredSkeletons.Add(tempSkel); } }
internal void AddDataPoint(KinectSkeleton data, JointType joint) { if (data.skeleton[joint].TrackingState == TrackingState.Tracked) { if (sequenceLength != 0) //Don't bother adding stuff to the sequence if its length should be 0 anyway { UpdateShoulderWidth(data.skeleton[JointType.ShoulderRight], data.skeleton[JointType.ShoulderLeft]); if (skeletonHistory.Count >= sequenceLength) { skeletonHistory.Dequeue(); } int clusterNumber = KMeans.FindNearestCluster(kCentroids, GetNormalizedRelativePosition(data.skeleton, joint)); skeletonHistory.Enqueue(clusterNumber); } } }
internal static KinectSkeleton DeepCopySkeleton(KinectSkeleton inSkel) { KinectSkeleton outSkel = new KinectSkeleton(); outSkel.leftHandClosed = inSkel.leftHandClosed; outSkel.Position = inSkel.Position; outSkel.rightHandClosed = inSkel.rightHandClosed; outSkel.SkeletonTrackingState = inSkel.SkeletonTrackingState; outSkel.sourceKinectID = inSkel.sourceKinectID; outSkel.TrackingId = inSkel.TrackingId; for (int i = 0; i < inSkel.skeleton.Count; i++) { outSkel.skeleton[i] = DeepCopyJoint(inSkel.skeleton[i]); } return(outSkel); }
private void stopButton_Click(object sender, RoutedEventArgs e) { recording = false; parent.MergedSkeletonChanged -= parent_MergedSkeletonChanged; if (isTrainingDataValid(trainingData)) { string temp = getTrainingDataName(); trainingDataNames.Add(temp); KinectSkeleton[] newList = new KinectSkeleton[trainingData.Count]; trainingData.CopyTo(newList); settings.trainingData.Add(new List <KinectSkeleton>(newList)); } else { HelperMethods.ShowErrorMessage("No Training Data", "Warning: No training data was recorded!", parent); } trainingData.Clear(); trainingSetsListBox.Items.Refresh(); stopButton.IsEnabled = false; testButton.IsEnabled = false; trainButton.IsEnabled = true; startButton.IsEnabled = true; }
private List <KinectSkeleton> SortSkeletons(List <KinectSkeleton> unsortedSkeletons, SkeletonSortMethod sortMethod) { if (sortMethod == SkeletonSortMethod.NoSort) { return(unsortedSkeletons); } else { //Seperate the tracked and untracked skeletons List <KinectSkeleton> trackedSkeletons = new List <KinectSkeleton>(); List <KinectSkeleton> untrackedSkeletons = new List <KinectSkeleton>(); for (int i = 0; i < unsortedSkeletons.Count; i++) { if (unsortedSkeletons[i].skeleton.TrackingState == SkeletonTrackingState.NotTracked) { untrackedSkeletons.Add(unsortedSkeletons[i]); } else { trackedSkeletons.Add(unsortedSkeletons[i]); } } if (sortMethod == SkeletonSortMethod.OriginXClosest || sortMethod == SkeletonSortMethod.OriginXFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && tempSkeleton.skeleton.Position.X < trackedSkeletons[insertIndex - 1].skeleton.Position.X) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginXFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.OriginYClosest || sortMethod == SkeletonSortMethod.OriginYFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && tempSkeleton.skeleton.Position.Y < trackedSkeletons[insertIndex - 1].skeleton.Position.Y) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginYFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.OriginZClosest || sortMethod == SkeletonSortMethod.OriginZFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && tempSkeleton.skeleton.Position.Z < trackedSkeletons[insertIndex - 1].skeleton.Position.Z) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginZFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.OriginEuclidClosest || sortMethod == SkeletonSortMethod.OriginEuclidFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; SkeletonPoint origin = new SkeletonPoint() { X = 0, Y = 0, Z = 0 }; double tempDistance = InterPointDistance(origin, trackedSkeletons[i].skeleton.Position); while (insertIndex > 0 && tempDistance < InterPointDistance(origin, trackedSkeletons[insertIndex - 1].skeleton.Position)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginEuclidFarthest) { trackedSkeletons.Reverse(); } } //TODO: Add feedback sort methods here //Add the untracked skeletons to the tracked ones before sending everything back trackedSkeletons.AddRange(untrackedSkeletons); return(trackedSkeletons); } }
private List <KinectSkeleton> SortSkeletons(List <KinectSkeleton> unsortedSkeletons, SkeletonSortMethod sortMethod) { if (sortMethod == SkeletonSortMethod.NoSort) { return(unsortedSkeletons); } else { //Seperate the tracked and untracked skeletons List <KinectSkeleton> trackedSkeletons = new List <KinectSkeleton>(); List <KinectSkeleton> untrackedSkeletons = new List <KinectSkeleton>(); for (int i = 0; i < unsortedSkeletons.Count; i++) { if (unsortedSkeletons[i].skeleton.TrackingState == SkeletonTrackingState.NotTracked) { untrackedSkeletons.Add(unsortedSkeletons[i]); } else { trackedSkeletons.Add(unsortedSkeletons[i]); } } if (sortMethod == SkeletonSortMethod.OriginXClosest || sortMethod == SkeletonSortMethod.OriginXFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.X) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.X)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginXFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.OriginYClosest || sortMethod == SkeletonSortMethod.OriginYFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Y) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Y)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginYFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.OriginZClosest || sortMethod == SkeletonSortMethod.OriginZFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Z) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Z)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginZFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.OriginEuclidClosest || sortMethod == SkeletonSortMethod.OriginEuclidFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; SkeletonPoint origin = new SkeletonPoint() { X = 0, Y = 0, Z = 0 }; double tempDistance = InterPointDistance(origin, trackedSkeletons[i].skeleton.Position); while (insertIndex > 0 && tempDistance < InterPointDistance(origin, trackedSkeletons[insertIndex - 1].skeleton.Position)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.OriginEuclidFarthest) { trackedSkeletons.Reverse(); } } else if (feedbackPosition != null) //Sort based on the feedback position, if it isn't null { if (sortMethod == SkeletonSortMethod.FeedbackXClosest || sortMethod == SkeletonSortMethod.FeedbackXFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.X - feedbackPosition.Value.X) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.X - feedbackPosition.Value.X)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.FeedbackXFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.FeedbackYClosest || sortMethod == SkeletonSortMethod.FeedbackYFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Y - feedbackPosition.Value.Y) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Y - feedbackPosition.Value.Y)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.FeedbackYFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.FeedbackZClosest || sortMethod == SkeletonSortMethod.FeedbackZFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; while (insertIndex > 0 && Math.Abs(tempSkeleton.skeleton.Position.Z - feedbackPosition.Value.Z) < Math.Abs(trackedSkeletons[insertIndex - 1].skeleton.Position.Z - feedbackPosition.Value.Z)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.FeedbackZFarthest) { trackedSkeletons.Reverse(); } } else if (sortMethod == SkeletonSortMethod.FeedbackEuclidClosest || sortMethod == SkeletonSortMethod.FeedbackEuclidFarthest) { //We only care about the tracked skeletons, so only sort those for (int i = 1; i < trackedSkeletons.Count; i++) { int insertIndex = i; KinectSkeleton tempSkeleton = trackedSkeletons[i]; SkeletonPoint feedPosition = new SkeletonPoint() { X = (float)feedbackPosition.Value.X, Y = (float)feedbackPosition.Value.Y, Z = (float)feedbackPosition.Value.Z }; double tempDistance = InterPointDistance(feedPosition, trackedSkeletons[i].skeleton.Position); while (insertIndex > 0 && tempDistance < InterPointDistance(feedPosition, trackedSkeletons[insertIndex - 1].skeleton.Position)) { trackedSkeletons[insertIndex] = trackedSkeletons[insertIndex - 1]; insertIndex--; } trackedSkeletons[insertIndex] = tempSkeleton; } if (sortMethod == SkeletonSortMethod.FeedbackEuclidFarthest) { trackedSkeletons.Reverse(); } } else { return(unsortedSkeletons); } } else { return(unsortedSkeletons); } //Add the untracked skeletons to the tracked ones before sending everything back trackedSkeletons.AddRange(untrackedSkeletons); return(trackedSkeletons); } }
private int FindSkeletonNumber(KinectSkeleton skeleton) { List <double> averageDistance = new List <double>(); for (int i = 0; i < filteredSkeletons.Count; i++) { //TODO: Should the time before desposal be changed? //Check if the skeleton has been updated in the last 5 seconds, and discard it if it hasn't //if (filteredSkeletons[i].AgeMS > 5000) //{ // filteredSkeletons.RemoveAt(i); // i--; //} //else //If the skeleton is still current, check the average distance to the merging skeletons joints //{ Point3D[] skelToCompare = filteredSkeletons[i].PredictPositionsOnly(0); double average = 0; int n = 0; for (int j = 0; j < skeleton.skeleton.Count; j++) { if (skeleton.skeleton[j].TrackingState == TrackingState.Tracked) { //Add the X distance to the average average += ((skelToCompare[j].X - skeleton.skeleton[j].Position.X) - average) / (n + 1); n++; //Add the Y distance to the average average += ((skelToCompare[j].Y - skeleton.skeleton[j].Position.Y) - average) / (n + 1); n++; //Add the Z distance to the average average += ((skelToCompare[j].Z - skeleton.skeleton[j].Position.Z) - average) / (n + 1); n++; } } //If any points were compared, add the average to the list, otherwise mark it as uncompared with NaN if (n > 0) { averageDistance.Add(average); } else { averageDistance.Add(double.NaN); } } //} //Go through the list of averages and find the lowest double absLowest = double.MaxValue; int lowestIdx = -1; //Initialize to -1 to mark that no matching skeleton was found for (int i = 0; i < averageDistance.Count; i++) { if (!double.IsNaN(averageDistance[i])) { double abs = Math.Abs(averageDistance[i]); if (abs < absLowest) { absLowest = abs; lowestIdx = i; } } } //TODO: is 0.3 m the best threshold to use? //If the best comparison has an average joint distance of < 0.3 m if (lowestIdx >= 0 && absLowest < 0.3) { return(lowestIdx); } else { return(-1); //-1 will be our error case to indicate that a matching filtered skeleton doesn't yet exist } }
internal void UpdateRecognizer(KinectSkeleton latestSkeleton) { }