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)
{
}