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;
}
