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