private static ImportedSkeleton CalculateSkeletonDerivative(List <ImportedSkeleton> query, int index, List <JointType> mostInformativeJoints)
{
var skeletonToReturn = new ImportedSkeleton();
if (index < query.Count - 1)
{
foreach (var jointType in mostInformativeJoints)
{
var currentMinusPrev = new JointRotation(
query[index].HiararchicalQuaternions[jointType].X - query[index - 1].HiararchicalQuaternions[jointType].X,
query[index].HiararchicalQuaternions[jointType].Y - query[index - 1].HiararchicalQuaternions[jointType].Y,
query[index].HiararchicalQuaternions[jointType].Z - query[index - 1].HiararchicalQuaternions[jointType].Z,
query[index].HiararchicalQuaternions[jointType].W - query[index - 1].HiararchicalQuaternions[jointType].W);
var nextMinusPrevDividedByTwo = new JointRotation(
(query[index + 1].HiararchicalQuaternions[jointType].X - query[index - 1].HiararchicalQuaternions[jointType].X) / 2,
(query[index + 1].HiararchicalQuaternions[jointType].Y - query[index - 1].HiararchicalQuaternions[jointType].Y) / 2,
(query[index + 1].HiararchicalQuaternions[jointType].Z - query[index - 1].HiararchicalQuaternions[jointType].Z) / 2,
(query[index + 1].HiararchicalQuaternions[jointType].W - query[index - 1].HiararchicalQuaternions[jointType].W) / 2);
var calculatedDerivative = new JointRotation(
(currentMinusPrev.X + nextMinusPrevDividedByTwo.X) / 2,
(currentMinusPrev.Y + nextMinusPrevDividedByTwo.Y) / 2,
(currentMinusPrev.Z + nextMinusPrevDividedByTwo.Z) / 2,
(currentMinusPrev.W + nextMinusPrevDividedByTwo.W) / 2
);
skeletonToReturn.HiararchicalQuaternions[jointType] = calculatedDerivative;
}
}
return(skeletonToReturn);
}
public ImportedSkeleton GetComputedAngles(Skeleton aSkeleton)
{
ImportedSkeleton skeleton = new ImportedSkeleton(aSkeleton);
#region Getting angles based on the projection of the coordinates
//Hashtable angles = GetAllAnglesRaw();
//foreach (JointType key in angles.Keys)
//{
// List<double> anglesInDouble = (List<double>)angles[key];
// SkeletonPoint point = new SkeletonPoint();
// if (!double.IsNaN(anglesInDouble[0]))
// {
// skeleton.HiararchicalQuaternions[key].XY = (float)anglesInDouble[0];
// }
// if (!double.IsNaN(anglesInDouble[1]))
// {
// skeleton.HiararchicalQuaternions[key].XZ = (float)anglesInDouble[1];
// }
// if (!double.IsNaN(anglesInDouble[2]))
// {
// skeleton.HiararchicalQuaternions[key].YZ = (float)anglesInDouble[2];
// }
//}
#endregion
return(skeleton);
}
public ImportedSkeleton GetComputedAngles(Skeleton aSkeleton)
{
ImportedSkeleton skeleton = new ImportedSkeleton(aSkeleton);
#region Getting angles based on the projection of the coordinates
//Hashtable angles = GetAllAnglesRaw();
//foreach (JointType key in angles.Keys)
//{
// List<double> anglesInDouble = (List<double>)angles[key];
// SkeletonPoint point = new SkeletonPoint();
// if (!double.IsNaN(anglesInDouble[0]))
// {
// skeleton.HiararchicalQuaternions[key].XY = (float)anglesInDouble[0];
// }
// if (!double.IsNaN(anglesInDouble[1]))
// {
// skeleton.HiararchicalQuaternions[key].XZ = (float)anglesInDouble[1];
// }
// if (!double.IsNaN(anglesInDouble[2]))
// {
// skeleton.HiararchicalQuaternions[key].YZ = (float)anglesInDouble[2];
// }
//}
#endregion
return skeleton;
}
public void AnglesExportToXML(ImportedSkeleton skeleton, List <JointType> SMIJ)
{
if (!TimeRecorded)
{
startTime = DateTime.Now;
TimeRecorded = true;
}
streamWriterXmlAngles.WriteLine("<skeleton>");
streamWriterXmlAngles.WriteLine("<frame>" + framesCount + "</frame>");
foreach (var joint in SMIJ)
{
streamWriterXmlAngles.Write("<" + joint + ">");
streamWriterXmlAngles.Write("<angles>");
streamWriterXmlAngles.Write("<Qx>" + skeleton.HiararchicalQuaternions[joint].X + "</Qx>");
streamWriterXmlAngles.Write("<Qy>" + skeleton.HiararchicalQuaternions[joint].Y + "</Qy>");
streamWriterXmlAngles.Write("<Qz>" + skeleton.HiararchicalQuaternions[joint].Z + "</Qz>");
streamWriterXmlAngles.Write("<Qw>" + skeleton.HiararchicalQuaternions[joint].W + "</Qw>");
streamWriterXmlAngles.Write("</angles>");
streamWriterXmlAngles.Write("<positions>");
streamWriterXmlAngles.Write("<x>" + skeleton.Joints[joint].Position.X + "</x>");
streamWriterXmlAngles.Write("<y>" + skeleton.Joints[joint].Position.Y + "</y>");
streamWriterXmlAngles.Write("<z>" + skeleton.Joints[joint].Position.Z + "</z>");
streamWriterXmlAngles.Write("</positions>");
streamWriterXmlAngles.WriteLine("</" + joint + ">");
}
framesCount++;
streamWriterXmlAngles.WriteLine("</skeleton>");
streamWriterXmlAngles.WriteLine();
}
public void AnglesExportToXML(ImportedSkeleton skeleton, List<JointType> SMIJ)
{
if (!TimeRecorded)
{
startTime = DateTime.Now;
TimeRecorded = true;
}
streamWriterXmlAngles.WriteLine("<skeleton>");
streamWriterXmlAngles.WriteLine("<frame>" + framesCount + "</frame>");
foreach (var joint in SMIJ)
{
streamWriterXmlAngles.Write("<" + joint + ">");
streamWriterXmlAngles.Write("<angles>");
streamWriterXmlAngles.Write("<Qx>" + skeleton.HiararchicalQuaternions[joint].X + "</Qx>");
streamWriterXmlAngles.Write("<Qy>" + skeleton.HiararchicalQuaternions[joint].Y + "</Qy>");
streamWriterXmlAngles.Write("<Qz>" + skeleton.HiararchicalQuaternions[joint].Z + "</Qz>");
streamWriterXmlAngles.Write("<Qw>" + skeleton.HiararchicalQuaternions[joint].W + "</Qw>");
streamWriterXmlAngles.Write("</angles>");
streamWriterXmlAngles.Write("<positions>");
streamWriterXmlAngles.Write("<x>" + skeleton.Joints[joint].Position.X + "</x>");
streamWriterXmlAngles.Write("<y>" + skeleton.Joints[joint].Position.Y + "</y>");
streamWriterXmlAngles.Write("<z>" + skeleton.Joints[joint].Position.Z + "</z>");
streamWriterXmlAngles.Write("</positions>");
streamWriterXmlAngles.WriteLine("</" + joint + ">");
}
framesCount++;
streamWriterXmlAngles.WriteLine("</skeleton>");
streamWriterXmlAngles.WriteLine();
}
public void AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
currentFrame++;
using (DepthImageFrame depth = e.OpenDepthImageFrame())
{
#region Regular checks
#region First
if (kinectSensor == null)
{
return;
}
GC.Collect();
#endregion
Skeleton first = Utility.GetFirstSkeleton.Get(e, allSkeletons);
#region Second
if (first == null)
{
return;
}
#endregion
#endregion
if (CurrentMode == Mode.FillingWindow)
{
window.Add(new ImportedSkeleton(first), ACCEPTABLE_WINDOW_SIZE);
if (window.Size >= ACCEPTABLE_WINDOW_SIZE)
{
CurrentMode = Mode.AlgorithmRunning;
}
}
if (CurrentMode == Mode.AlgorithmRunning)
{
if (currentFrame % 2 == 0)
{
foreach (var activity in Activities)
{
double overallResult = 0.0;
foreach (var activityRecord in activity.Recordings)
{
var activityRecordResult = 0.0;
if (AlgorithmType == AlgorithmTypes.DTW)
{
activityRecordResult = DTW(activityRecord, window, DynamicTimeWarpingCalculationType.Standart, StepPattern, true);
}
else if (AlgorithmType == AlgorithmTypes.DLM)
{
activityRecordResult = ElasticMatchingWithFreedomDegree.CompareActivities(activityRecord, window);
}
overallResult += activityRecordResult;
}
overallResult /= activity.Recordings.Count;
var currentAlgorithmAcceptableActionSimilarity = 0;
if (AlgorithmType == AlgorithmTypes.DTW)
{
currentAlgorithmAcceptableActionSimilarity = AcceptableActionSimilarity.DTW;
}
else if (AlgorithmType == AlgorithmTypes.DLM)
{
currentAlgorithmAcceptableActionSimilarity = AcceptableActionSimilarity.DLM;
}
if (overallResult <= currentAlgorithmAcceptableActionSimilarity && !activityRecognizingStartedTriggered)
{
activityRecognizingStartedTriggered = true;
activityRecognizingEndedTriggered = false;
ActivityRecognizingStarted.Invoke(this, new ActivityRecognizingEventArgs(activity, overallResult));
recognizedActivityName = activity.Name;
}
else if (overallResult > currentAlgorithmAcceptableActionSimilarity && !activityRecognizingEndedTriggered && recognizedActivityName == activity.Name)
{
activityRecognizingEndedTriggered = true;
activityRecognizingStartedTriggered = false;
ActivityRecognizingEnded.Invoke(this, new ActivityRecognizingEventArgs(activity, overallResult));
}
Console.WriteLine(overallResult);
}
Console.WriteLine();
}
CurrentMode = Mode.FillingWindow;
}
if (CurrentMode == Mode.ToSelectMainSkeleton)
{
CurrentMode = Mode.None;
mainSkeleton = first;
JointAnglesManager jointManager = new JointAnglesManager(first);
mainSkeletonWithAngles = jointManager.GetComputedAngles(mainSkeleton);
}
if (CurrentMode == Mode.ComparingSkeletons)
{
///Debug data - Most informative joints
List <JointType> joints = new List <JointType>();
joints.Add(JointType.ShoulderLeft);
joints.Add(JointType.ShoulderRight);
joints.Add(JointType.Head);
joints.Add(JointType.ElbowLeft);
joints.Add(JointType.ElbowRight);
///
Skeleton secondarySkeleton = first;
var jointManager = new JointAnglesManager(secondarySkeleton);
var secondarySkeletonWithAngles = jointManager.GetComputedAngles(secondarySkeleton);
var result = SkeletonComparer.CompareWithSMIJ(mainSkeletonWithAngles, secondarySkeletonWithAngles, joints);
Console.WriteLine("Skeleton comparison: {0}", result);
if (result < ACCEPTABLE_SKELETON_SIMILARITY)
{
PoseRecognizedEventArgs args = new PoseRecognizedEventArgs(result);
PoseReconized.Invoke(this, args);
}
}
}
}
public void Add(ImportedSkeleton skel, int maxSize)
{
windowFixedSize = maxSize;
Frames.Enqueue(skel);
FixFramesLenght();
}
public static double CompareActivities(ActivityRecord record, ActivityWindow window, DynamicTimeWarpingCalculationType dtwType, DynamicTimeWarpingPathTypes pathType = DynamicTimeWarpingPathTypes.Standart, bool toUseSakoeChibaBand = false, double bandWidthInProcentage = 0.1)
{
double similarity = 0.0f;
double[,] DTW = new double[record.Frames.Count, window.Frames.Count];
for (int i = 1; i < window.Frames.Count; i++)
{
DTW[0, i] = double.PositiveInfinity;
}
for (int i = 1; i < record.Frames.Count; i++)
{
DTW[i, 0] = double.PositiveInfinity;
}
DTW[0, 0] = 0;
var windowPresentedByImportedSkeletons = new List <ImportedSkeleton>();
foreach (var skeleton in window.Frames)
{
windowPresentedByImportedSkeletons.Add(new ImportedSkeleton(skeleton));
}
int bandWidth = (int)(windowPresentedByImportedSkeletons.Count * bandWidthInProcentage);
for (int i = 1; i < record.Frames.Count; i++)
{
for (int j = 1; j < windowPresentedByImportedSkeletons.Count; j++)
{
if (toUseSakoeChibaBand)
{
int currentCellOnMiddleDiagonal = (int)((j * windowPresentedByImportedSkeletons.Count) / record.Frames.Count);
if (j > currentCellOnMiddleDiagonal - bandWidth && j < currentCellOnMiddleDiagonal + bandWidth) // Checking if the current cell is in the range
{
if (dtwType == DynamicTimeWarpingCalculationType.Standart)
{
similarity = SkeletonComparer.CompareWithSMIJ(record.Frames[i], windowPresentedByImportedSkeletons[j], record.MostInformativeJoints);
}
else if (dtwType == DynamicTimeWarpingCalculationType.Derivative)
{
if (i != record.Frames.Count)
{
ImportedSkeleton mainSkeletonDerivatives = CalculateSkeletonDerivative(record.Frames, i, record.MostInformativeJoints);
ImportedSkeleton secondarySkeletonDerivatives = CalculateSkeletonDerivative(windowPresentedByImportedSkeletons, i, record.MostInformativeJoints);
similarity = SkeletonComparer.CompareWithSMIJ(mainSkeletonDerivatives, secondarySkeletonDerivatives, record.MostInformativeJoints);
}
}
}
}
else //Start a DTW search without using Sakoe-Chiba band
{
if (dtwType == DynamicTimeWarpingCalculationType.Derivative)
{
if (i != record.Frames.Count)
{
ImportedSkeleton mainSkeletonDerivatives = CalculateSkeletonDerivative(record.Frames, i, record.MostInformativeJoints);
ImportedSkeleton secondarySkeletonDerivatives = CalculateSkeletonDerivative(windowPresentedByImportedSkeletons, i, record.MostInformativeJoints);
similarity = SkeletonComparer.CompareWithSMIJ(mainSkeletonDerivatives, secondarySkeletonDerivatives, record.MostInformativeJoints);
}
}
else if (dtwType == DynamicTimeWarpingCalculationType.Standart)
{
similarity = SkeletonComparer.CompareWithSMIJ(record.Frames[i], windowPresentedByImportedSkeletons[j], record.MostInformativeJoints);
}
}
FillDTWTableCell(pathType, DTW, i, j, similarity);
}
}
return(DTW[record.Frames.Count - 1, window.Frames.Count - 1] / record.MostInformativeJoints.Count);
}
