public static void RenderDepthPixels(MainWindow mainWindow, CameraSpacePoint[] pointCloud) { byte[] intensityMap = ImageUtils.CalculateIntensityFromCameraSpacePoints(pointCloud); DrawCanvasOnMap(intensityMap); ClearCanvas(); GlobVar.IntensityBitmap.WritePixels( new Int32Rect(0, 0, GlobVar.IntensityBitmap.PixelWidth, GlobVar.IntensityBitmap.PixelHeight), intensityMap, GlobVar.IntensityBitmap.PixelWidth * GlobVar.IntensityBitmap.Format.BitsPerPixel / 8, 0); var canvas = new Canvas { Width = 700, Height = 580 }; if (BodyUtils.HasBodyTracking() & Logger.DrawEnergyValue) { DrawEnergyValue(canvas); } canvas.Background = new ImageBrush(GlobVar.IntensityBitmap); mainWindow.Content = canvas; mainWindow.Show(); }
/// <summary> /// Adds the torsopoints to body /// </summary> /// /// <remarks> /// If the current body id had tracking in previous frames, the average torso location the last frames is saved. /// </remarks> public void AddTorso(List <int> torsoPoints) { CameraSpacePoint currentAvg = BodyUtils.CalculateAveragePointFromValidPoints(torsoPoints); if (BodyUtils.HasTorsoTracking(Id)) { CameraSpacePoint avgLastFrames = BodyUtils.GetAverageTorsoLocationLastFrames(Id); if (GlobUtils.GetEuclideanDistance(avgLastFrames, currentAvg) < Thresholds.LastFramesTorsoMaxDistance) { Torso torso = new Torso(currentAvg, avgLastFrames); Torso = torso; } else { Torso torso = new Torso(currentAvg, new CameraSpacePoint() { X = float.NaN, Y = float.NaN, Z = float.NaN }); Torso = torso; } } else { Torso torso = new Torso(currentAvg, new CameraSpacePoint() { X = float.NaN, Y = float.NaN, Z = float.NaN }); Torso = torso; } }
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); }
public int AddHeadPixels(List <int> headPixels) { HeadPointIndexes = headPixels; CenterPoint = BodyUtils.CalculateAveragePointFromValidPoints(headPixels); var depthSpacePoint = GlobVar.CoordinateMapper.MapCameraPointToDepthSpace(CenterPoint); CenterPointIndex = GlobUtils.GetIndex((int)Math.Round(depthSpacePoint.X) / 2, (int)Math.Round(depthSpacePoint.Y) / 2); return(CenterPointIndex); }
/// <summary> /// Creates a geodesic graph of the foreground. Points in cameraspace that are closer than a threshold are connected in the graph. /// Also, the points need to be closer than threshold to the already validated candidate head points. /// </summary> public static void CreateGeodesicGraph(CameraSpacePoint[] pointCloud) { GeodesicGraph = new Dictionary <int, Dictionary <int, float> >(); const float maxDepth = GlobVar.MaxSensingDepth; for (var i = 0; i < GlobVar.ScaledFrameLength; i++) { if (pointCloud[i].Z != maxDepth && !float.IsInfinity(pointCloud[i].X) && !float.IsInfinity(pointCloud[i].Y)) { var neighbourList = GlobUtils.GetNeighbour3X3IndexList(i); var neighbourNodes = new Dictionary <int, float>(); foreach (var neighbour in neighbourList) { if (neighbour != -1) { if (pointCloud[neighbour].Z != maxDepth) { var dist = GlobUtils.GetEuclideanDistance(neighbour, i); if (dist < Thresholds.GeodesicGraphMaxDistanceBetweenPoints && BodyUtils.GetDistanceToClosestHeadCandidate(neighbour) < Thresholds.GeodesicGraphMaxDistanceToCandidates) { neighbourNodes.Add(neighbour, dist); } } } } if (neighbourNodes.Count > 0) { GeodesicGraph.Add(i, neighbourNodes); } } } if (Logger.ShowGeodesicGraph) { foreach (var v in GeodesicGraph) { GlobVar.GraphicsCanvas[v.Key] = 255; } } }
/// <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> /// Adds the handpoints to the body. /// </summary> /// /// <remarks> /// The new candidate hands are compared to average location of previous hands for bodies with same id, to identify if the candidate hands are left or right. /// </remarks> public void AddHands(List <int> handPoints) { List <CameraSpacePoint> handCenterPoints = BodyUtils.GroupHandPoints(handPoints); Hand[] newHands = new Hand[2]; var averageHandLocationsLastFrames = BodyUtils.GetAverageHandLocationsLastFrames(Id); CameraSpacePoint avgFirstHandPoint = averageHandLocationsLastFrames[0]; CameraSpacePoint avgSecondHandPoint = averageHandLocationsLastFrames[1]; var distancesToHandAverages = BodyUtils.GetDistancesToHandAveragesLastFrames(handCenterPoints, avgFirstHandPoint, avgSecondHandPoint); var sortedDistancesToHandAverages = distancesToHandAverages.OrderBy(kvp => kvp.Value); if (BodyUtils.HasFirstHandTracking(Id) || BodyUtils.HasSecondHandTracking(Id)) { bool[] handsUsed = { false, false }; foreach (var distance in sortedDistancesToHandAverages) { if (distance.Key == 0) { if (BodyUtils.HasFirstHandTracking(Id)) { if (distancesToHandAverages[distance.Key] < Thresholds.LastFramesHandMaxDistance) { Hand hand = new Hand(handCenterPoints[0], 0, avgFirstHandPoint); if (newHands[0] == null && !handsUsed[0]) { newHands[0] = hand; handsUsed[0] = true; } } } else { Hand hand = new Hand(handCenterPoints[0], 0, avgFirstHandPoint); if (newHands[0] == null && !handsUsed[0]) { newHands[0] = hand; handsUsed[0] = true; } } } else if (distance.Key == 1) { if (BodyUtils.HasSecondHandTracking(Id)) { if (distancesToHandAverages[distance.Key] < Thresholds.LastFramesHandMaxDistance) { Hand hand = new Hand(handCenterPoints[0], 1, avgSecondHandPoint); if (newHands[1] == null && !handsUsed[0]) { newHands[1] = hand; handsUsed[0] = true; } } } else { Hand hand = new Hand(handCenterPoints[0], 1, avgSecondHandPoint); if (newHands[1] == null && !handsUsed[0]) { newHands[1] = hand; handsUsed[0] = true; } } } else if (distance.Key == 2) { if (BodyUtils.HasFirstHandTracking(Id)) { if (distancesToHandAverages[distance.Key] < Thresholds.LastFramesHandMaxDistance) { Hand hand = new Hand(handCenterPoints[1], 0, avgFirstHandPoint); if (newHands[0] == null && !handsUsed[1]) { newHands[0] = hand; handsUsed[1] = true; } } } else { Hand hand = new Hand(handCenterPoints[1], 0, avgFirstHandPoint); if (newHands[0] == null && !handsUsed[1]) { newHands[0] = hand; handsUsed[1] = true; } } } else if (distance.Key == 3) { if (BodyUtils.HasSecondHandTracking(Id)) { if (distancesToHandAverages[distance.Key] < Thresholds.LastFramesHandMaxDistance) { Hand hand = new Hand(handCenterPoints[1], 1, avgSecondHandPoint); if (newHands[1] == null && !handsUsed[1]) { newHands[1] = hand; handsUsed[1] = true; } } } else { Hand hand = new Hand(handCenterPoints[1], 1, avgSecondHandPoint); if (newHands[1] == null && !handsUsed[1]) { newHands[1] = hand; handsUsed[1] = true; } } } } } else { for (int i = 0; i < handCenterPoints.Count; i++) { newHands[i] = new Hand(handCenterPoints[i], i, new CameraSpacePoint() { X = float.NaN, Y = float.NaN, Z = float.NaN }); } } Hands = newHands; }