/// <summary>
/// Groups the hand points if closer than threshold.
/// </summary>
public static List <CameraSpacePoint> GroupHandPoints(List <int> handPointsIndexes)
{
List <CameraSpacePoint> handPoints = new List <CameraSpacePoint>();
for (int i = 0; i < handPointsIndexes.Count; i++)
{
handPoints.Add(GlobVar.SubtractedFilteredPointCloud[handPointsIndexes[i]]);
}
DisjointSet3D disjointSetHandPoints = new DisjointSet3D(handPoints);
for (int i = 0; i < handPoints.Count; i++)
{
for (int j = 0; j < handPoints.Count; j++)
{
if (i == j)
{
continue;
}
;
if (GlobUtils.GetEuclideanDistance(handPoints[i], handPoints[j]) < Thresholds.HandPointGroupingMaxDistance)
{
disjointSetHandPoints.Union(i, j);
}
}
}
List <CameraSpacePoint> handCenterPoints = FilterHandCandidates(disjointSetHandPoints);
return(handCenterPoints);
}
/// <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;
}
}
public static float GetDistanceToClosestHeadCandidate(int indexPoint)
{
float minDistance = float.MaxValue;
foreach (var head in GlobVar.ValidatedCandidateHeads)
{
var headIndex = head.HighestPointIndex;
float currentDistance = GlobUtils.GetEuclideanDistance(headIndex, indexPoint);
if (currentDistance < minDistance)
{
minDistance = currentDistance;
}
}
return(minDistance);
}
public static Dictionary <int, float> GetDistancesToHandAveragesLastFrames(List <CameraSpacePoint> handCenterPoints, CameraSpacePoint avgFirstHandPoint,
CameraSpacePoint avgSecondHandPoint)
{
Dictionary <int, float> distancesToHandAverages = new Dictionary <int, float>();
int index = 0;
foreach (var handCenterPoint in handCenterPoints)
{
float distToFirstHandAverage = GlobUtils.GetEuclideanDistance(avgFirstHandPoint, handCenterPoint);
distancesToHandAverages.Add(index, distToFirstHandAverage);
index++;
float distToSecondHandAverage = GlobUtils.GetEuclideanDistance(avgSecondHandPoint, handCenterPoint);
distancesToHandAverages.Add(index, distToSecondHandAverage);
index++;
}
return(distancesToHandAverages);
}
/// <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;
}
}
}
private static Dictionary <Tuple <Head, int>, float> GetDistancesFromCandidateHeadsToRecentHeads(List <Head> validatedCandidateHeads,
Dictionary <int, CameraSpacePoint> avgLocationsRecentHeads)
{
var distancesFromCandidateHeadsToRecentHeads = new Dictionary <Tuple <Head, int>, float>();
foreach (var candidateHead in validatedCandidateHeads)
{
foreach (var avgLocationRecentHead in avgLocationsRecentHeads)
{
var assignment = new Tuple <Head, int>(candidateHead, avgLocationRecentHead.Key);
var distanceToCandidate = GlobUtils.GetEuclideanDistance(candidateHead.CenterPoint,
avgLocationRecentHead.Value);
if (distanceToCandidate < Thresholds.LastFramesHeadMaxDistance)
{
distancesFromCandidateHeadsToRecentHeads.Add(assignment, distanceToCandidate);
}
}
}
return(distancesFromCandidateHeadsToRecentHeads);
}
