private static Vector3D[] CalculateDisplacementHands(Body[] bodies) { float displacementFirstHandX = 0; float displacementFirstHandY = 0; float displacementFirstHandZ = 0; float displacementSecondHandX = 0; float displacementSecondHandY = 0; float displacementSecondHandZ = 0; for (int i = 1; i < bodies.Length; i++) { if (bodies[i].Hands[0] != null && bodies[i - 1].Hands[0] != null && !float.IsNaN(bodies[i].Hands[0].AvgCenterPointLastFrames.X) && !float.IsNaN(bodies[i - 1].Hands[0].AvgCenterPointLastFrames.X)) { displacementFirstHandX = Math.Abs(bodies[i].Hands[0].AvgCenterPointLastFrames.X - bodies[i - 1].Hands[0].AvgCenterPointLastFrames.X); displacementFirstHandY = Math.Abs(bodies[i].Hands[0].AvgCenterPointLastFrames.Y - bodies[i - 1].Hands[0].AvgCenterPointLastFrames.Y); displacementFirstHandZ = Math.Abs(bodies[i].Hands[0].AvgCenterPointLastFrames.Z - bodies[i - 1].Hands[0].AvgCenterPointLastFrames.Z); } if (bodies[i].Hands[1] != null && bodies[i - 1].Hands[1] != null && !float.IsNaN(bodies[i].Hands[1].AvgCenterPointLastFrames.X) && !float.IsNaN(bodies[i - 1].Hands[1].AvgCenterPointLastFrames.X)) { displacementSecondHandX = Math.Abs(bodies[i].Hands[1].AvgCenterPointLastFrames.X - bodies[i - 1].Hands[1].AvgCenterPointLastFrames.X); displacementSecondHandY = Math.Abs(bodies[i].Hands[1].AvgCenterPointLastFrames.Y - bodies[i - 1].Hands[1].AvgCenterPointLastFrames.Y); displacementSecondHandZ = Math.Abs(bodies[i].Hands[1].AvgCenterPointLastFrames.Z - bodies[i - 1].Hands[1].AvgCenterPointLastFrames.Z); } } var displacementFirstHand = new Vector3D(displacementFirstHandX,displacementFirstHandY,displacementFirstHandZ); var displacementSecondHand = new Vector3D(displacementSecondHandX,displacementSecondHandY,displacementSecondHandZ); return new[]{displacementFirstHand,displacementSecondHand}; }
/// <summary> /// Draw bodyelements in detection frame for debugging purposes. /// </summary> public static void Draw(Body body) { if (ShowHandCenterPoint) { Hand firstHand = body.Hands[0]; if (firstHand != null) { GraphicsUtils.DrawPoint(firstHand.CenterPoint); } Hand secondHand = body.Hands[1]; if (secondHand != null) { GraphicsUtils.DrawPoint(secondHand.CenterPoint); } } if (ShowHandAvgPoint) { Hand firstHand = body.Hands[0]; if (firstHand != null && !float.IsNaN(firstHand.AvgCenterPointLastFrames.Z)) { GraphicsUtils.DrawPoint(firstHand.AvgCenterPointLastFrames); } Hand secondHand = body.Hands[1]; if (secondHand != null && !float.IsNaN(secondHand.AvgCenterPointLastFrames.Z)) { GraphicsUtils.DrawPoint(secondHand.AvgCenterPointLastFrames); } } if (ShowTorsoCenterPoint) { if (body.Torso != null && !float.IsNaN(body.Torso.CenterPoint.Z)) { GraphicsUtils.DrawPoint(body.Torso.CenterPoint); } } if (ShowTorsoAvgCenterPoint) { if (body.Torso != null && !float.IsNaN(body.Torso.AvgCenterPoint.Z)) { GraphicsUtils.DrawPoint(body.Torso.AvgCenterPoint); } } if (ShowHeadCenterPoint) { GraphicsUtils.DrawPoint(body.Head.CenterPoint); } }
/// <summary> /// Attributes the body object with the calculated mechanical energy-use since last frame. /// </summary> public static void CalculateBodyMechanicalEnergy(Body body) { bool handTracking = false; List<Body> lastBodies = BodyUtils.GetBodiesWithIdFromHistory(body.Id); if (lastBodies.Count < 1) { body.EnergyLevel = 0; return; } Body[] lastTwoBodies = new Body[2]; lastTwoBodies[0] = lastBodies[lastBodies.Count - 1]; lastTwoBodies[1] = body; double totalPotential = 0; double totalKinetic = 0; Vector3D headDisplacement = CalculateDisplacementHead(lastTwoBodies); totalPotential += CalculatePotentialEnergyChange(headDisplacement, HeadMass); totalKinetic += CalculateKineticEnergyChange(headDisplacement, HeadMass, lastTwoBodies); Vector3D torsoDisplacement = CalculateDisplacementTorso(lastTwoBodies); totalPotential += CalculatePotentialEnergyChange(torsoDisplacement, TorsoMass); totalKinetic += CalculateKineticEnergyChange(torsoDisplacement, TorsoMass, lastTwoBodies); if (handTracking) { Vector3D[] handDisplacement = CalculateDisplacementHands(lastTwoBodies); totalPotential += CalculatePotentialEnergyChange(handDisplacement[0], ForeArmMass); totalKinetic += CalculateKineticEnergyChange(handDisplacement[0], ForeArmMass, lastTwoBodies); totalPotential += CalculatePotentialEnergyChange(handDisplacement[1], ForeArmMass); totalKinetic += CalculateKineticEnergyChange(handDisplacement[1], ForeArmMass, lastTwoBodies); } body.EnergyLevel = totalKinetic + totalPotential; }
/// <summary> /// Uses Dijkstra to find the shortest geodesic path from the top of the head to the rest of the body. The body parts /// torso and hands are classified as the points from the geodesic map that fall into bins with a certain distance from /// the top of the head. /// </summary> /// <returns>Returns a dictionary with the heads as keys and labels as values. If head isn't identified, the label is -1</returns> public static void AddBodyRegions(Dictionary<int, Dictionary<int, float>> geodesicGraph, Head head, Body body) { var startIndex = head.HighestPointIndex; if (!geodesicGraph.ContainsKey(startIndex)) { startIndex = GeodesicUtils.GetClosestValidNeighbourInGeodesicGraph(startIndex); } if (startIndex == -1) { return; } var minDistances = Dijkstra.Perform(startIndex, geodesicGraph); var handPoints = new List<int>(); var torsoPoints = new List<int>(); foreach (var minDistance in minDistances) { if (minDistance.Value < 1.1f) { // Include body in current heatcanvas GlobVar.HeatCanvas[minDistance.Key] = body.Id; } if (minDistance.Value < 0.2f) { if (Logger.ShowHeadRegionPixels) { GlobVar.GraphicsCanvas[minDistance.Key] = 255; } torsoPoints.Add(minDistance.Key); } else if (minDistance.Value < 0.6f && minDistance.Value > 0.2f) { if (Logger.ShowTorsoPixels) { GlobVar.GraphicsCanvas[minDistance.Key] = 200; } torsoPoints.Add(minDistance.Key); } else if (minDistance.Value < 1.2f && minDistance.Value > 0.9f) { if (Logger.ShowHandPixels) { GlobVar.GraphicsCanvas[minDistance.Key] = 150; } handPoints.Add(minDistance.Key); } } if (torsoPoints.Count > Thresholds.TorsoPointMinCount) { body.AddTorso(torsoPoints); } if (handPoints.Count > Thresholds.HandPointMinCount) { body.AddHands(handPoints); } }
private List<Body> CreateBodies(TimeSpan frameRelativeTime, List<Head> validatedHeads) { _stopwatch.Restart(); Dictionary<Head, int> identifiedHeadsWithLabels = BodyUtils.IdentifyHeads(validatedHeads); var bodies = new List<Body>(); foreach (var identifiedHead in identifiedHeadsWithLabels) { Stopwatch stopwatchBody = new Stopwatch(); stopwatchBody.Start(); Body body; int bodyId = identifiedHead.Value; Head head = identifiedHead.Key; if (bodyId == -1) { GlobVar.MaxBodyCount++; int newBodyId = GlobVar.MaxBodyCount; body = new Body(newBodyId, frameRelativeTime); body.AddHead(head); bodies.Add(body); BodyUtils.AddBodyRegions(GeodesicUtils.GeodesicGraph, head, body); } else { // Attribute head with average centerpoint in from the last // frames. head.AvgCenterPoint = BodyUtils.GetAverageHeadLocationLastFrames(bodyId); if (Logger.ShowHeadAvgCenterPoint) { GraphicsUtils.DrawPoint(head.AvgCenterPoint); } body = new Body(bodyId, frameRelativeTime); body.AddHead(head); bodies.Add(body); BodyUtils.AddBodyRegions(GeodesicUtils.GeodesicGraph, head, body); } Logger.Draw(body); EnergyUtils.CalculateBodyMechanicalEnergy(body); } if (Logger.LogTimeCreateBodies) { _stopwatch.Stop(); Console.WriteLine("CreateBodies: {0}", _stopwatch.ElapsedMilliseconds); } return bodies; }
private static double CalculateKineticEnergyChange(Vector3D displacement, double mass, Body[] bodies) { if (double.IsNaN(displacement.Z)) { return 0; } TimeSpan timeStampStart = bodies[0].TimeStamp; TimeSpan timeStampStop = bodies[1].TimeStamp; double deltaTime = timeStampStop.Subtract(timeStampStart).TotalSeconds; if (deltaTime == 0) { return 0; } double velocityX = displacement.X/deltaTime; double velocityY = displacement.Y/deltaTime; double kineticEnergyX = 0.5*mass*velocityX*velocityX; double kineticEnergyY = 0.5*mass*velocityY*velocityY; return kineticEnergyX + kineticEnergyY; }
private static Vector3D CalculateDisplacementTorso(Body[] bodies) { float displacementX = 0; float displacementY = 0; float displacementZ = 0; for (int i = 1; i < bodies.Length; i++) { if (bodies[i].Torso != null && bodies[i-1].Torso != null && !float.IsNaN(bodies[i].Torso.AvgCenterPoint.X) && !float.IsNaN(bodies[i - 1].Torso.AvgCenterPoint.X)) { displacementX += Math.Abs(bodies[i].Torso.AvgCenterPoint.X - bodies[i - 1].Torso.AvgCenterPoint.X); displacementY += Math.Abs(bodies[i].Torso.AvgCenterPoint.Y - bodies[i - 1].Torso.AvgCenterPoint.Y); displacementZ += Math.Abs(bodies[i].Torso.AvgCenterPoint.Z - bodies[i - 1].Torso.AvgCenterPoint.Z); } } return new Vector3D(displacementX, displacementY, displacementZ); }
private static Vector3D CalculateDisplacementHead(Body[] bodies) { float displacementX = 0; float displacementY = 0; float displacementZ = 0; displacementX += Math.Abs(bodies[0].Head.AvgCenterPoint.X - bodies[1].Head.AvgCenterPoint.X); displacementY += Math.Abs(bodies[0].Head.AvgCenterPoint.Y - bodies[1].Head.AvgCenterPoint.Y); displacementZ += Math.Abs(bodies[0].Head.AvgCenterPoint.Z - bodies[1].Head.AvgCenterPoint.Z); return new Vector3D(displacementX, displacementY, displacementZ); }