/// <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> /// Recursively searches for the highest point connected to the startindex. The search stops when the maximum searchdepth is reached. /// </summary> /// <param name="currentIndex"></param> /// <param name="searchDepth"></param> /// <returns></returns> public static int GetHighestConnectingPoint(int currentIndex, int searchDepth) { float currentDepth = GlobVar.SubtractedFilteredPointCloud[currentIndex].Z; int[] neighbours = GlobUtils.GetNeighbour5X5IndexList(currentIndex); int highestNeighbourIndex = currentIndex; float shallowestNeighbourDepth = currentDepth; for (int i = 0; i < neighbours.Length; i++) { int neighbourIndex = neighbours[i]; if (!GlobUtils.BoundaryCheck(neighbourIndex)) { continue; } CameraSpacePoint neighbour = GlobVar.SubtractedFilteredPointCloud[neighbourIndex]; if (neighbour.Z < shallowestNeighbourDepth && !float.IsInfinity(neighbour.X) && !float.IsInfinity(neighbour.Y)) { shallowestNeighbourDepth = neighbour.Z; highestNeighbourIndex = neighbourIndex; } } if (highestNeighbourIndex == currentIndex || searchDepth == 0) { return(currentIndex); } searchDepth--; return(GetHighestConnectingPoint(highestNeighbourIndex, searchDepth)); }
/// <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 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); }
public static void DrawPoint(Point point) { int x = point.X; int y = point.Y; int i; i = GlobUtils.GetIndex(x, y); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x + 1, y - 1); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x + 1, y); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x + 1, y + 1); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x - 1, y - 1); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x - 1, y); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x - 1, y + 1); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x, y + 1); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } i = GlobUtils.GetIndex(x, y - 1); if (GlobUtils.BoundaryCheck(i)) { GlobVar.GraphicsCanvas[i] = (byte)255; } }
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> /// Makes sure the filtered pixel also has valid x- and y-coordinates. /// </summary> /// <remarks> /// Helper function to the 3x3 median filter. /// </remarks> private static float[] GetClosestValidXyValues(int currentIndex) { var neighbours = GlobUtils.GetNeighbour3X3IndexList(currentIndex); var validValues = new float[] { float.PositiveInfinity, float.PositiveInfinity }; for (int i = 0; i < neighbours.Length; i++) { var point = GlobVar.SubtractedFilteredPointCloud[i]; var x = point.X; var y = point.Y; if (!float.IsInfinity(x) && !float.IsInfinity(y)) { validValues[0] = x; validValues[1] = y; } } return(validValues); }
/// <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; } } }
public static void DrawRectangle(IndexRectangle rect) { Point a = GlobUtils.GetPoint(rect.A); Point b = GlobUtils.GetPoint(rect.B); Point c = GlobUtils.GetPoint(rect.C); int topLength = b.X - a.X; int sideLength = c.Y - a.Y; for (int i = 0; i < topLength; i++) { GlobVar.GraphicsCanvas[GlobUtils.GetIndex(a.X + i, a.Y)] = (byte)120; GlobVar.GraphicsCanvas[GlobUtils.GetIndex(c.X + i, c.Y)] = (byte)120; } for (int i = 0; i < sideLength; i++) { GlobVar.GraphicsCanvas[GlobUtils.GetIndex(a.X, a.Y + i)] = (byte)120; GlobVar.GraphicsCanvas[GlobUtils.GetIndex(b.X, b.Y + i)] = (byte)120; } }
/// <summary> /// Connected component labeling. Height difference between connected pixels are used as evaluation criteria. /// </summary> /// <param name="startIndex">start index of the labeling</param> /// <param name="maxPixels">maximum amount of pixels allowed in resulting labeled component</param> /// <returns>Returns a list of connected headpoints</returns> public static List <int> ConnectedComponentLabeling(int startIndex, int maxPixels) { var q = new Queue <int>(); q.Enqueue(startIndex); List <int> headPixels = new List <int>(); headPixels.Add(startIndex); while (q.Count > 0) { if (maxPixels < 1) { break; } int currentIndex = q.Dequeue(); CameraSpacePoint currentPoint = GlobVar.SubtractedFilteredPointCloud[currentIndex]; int[] neighbours = GlobUtils.GetNeighbour3X3IndexList(currentIndex); for (int i = 0; i < neighbours.Length; i++) { int neighbourIndex = neighbours[i]; if (neighbourIndex == -1) { continue; } CameraSpacePoint neighbourPoint = GlobVar.SubtractedFilteredPointCloud[neighbourIndex]; if (!headPixels.Contains(neighbourIndex) && Thresholds.ClassificationLabelingMaxHeightBetweenPoints > GlobUtils.GetHeightDifference(currentPoint, neighbourPoint)) { q.Enqueue(neighbourIndex); headPixels.Add(neighbourIndex); maxPixels--; } } } return(headPixels); }
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); }
/// <summary> /// Groups the candidates if the distance between them in the XY-plane is below threshold. /// </summary> private static DisjointSet GroupCandidates(List <int> candidatePoints) { DisjointSet groupedSet = new DisjointSet(candidatePoints); for (int i = 0; i < candidatePoints.Count; i++) { for (int j = 0; j < candidatePoints.Count; j++) { if (i == j) { continue; } ; if (GlobUtils.GetEuclideanDistanceXYPlane(candidatePoints[i], candidatePoints[j]) < Thresholds.HaarMaxDistanceBetweenCandidates) { groupedSet.Union(i, j); } } } return(groupedSet); }
///<remarks> /// Due to imprecisions in the coordinate mapping, if the startIndex for the search in the geodesic graph doesn't exist, the closest neighbour is used. A breadth first search is used. /// </remarks> public static int GetClosestValidNeighbourInGeodesicGraph(int startIndex) { var q = new Queue <int>(); q.Enqueue(startIndex); var maxPixels = 30; while (q.Count > 0) { if (maxPixels < 1) { return(-1); } var currentIndex = q.Dequeue(); var neighbours = GlobUtils.GetNeighbour3X3IndexList(currentIndex); for (var i = 0; i < neighbours.Length; i++) { var neighbourIndex = neighbours[i]; if (neighbourIndex == -1) { continue; } if (GeodesicGraph.ContainsKey(neighbourIndex)) { if (GeodesicGraph[neighbourIndex].Count > 1) { return(neighbourIndex); } } q.Enqueue(neighbourIndex); maxPixels--; } } return(-1); }
/// <summary> /// Groups points where the distance between them in the euclidean XY-plane is below threshold. The highest point is kept. /// </summary> public static List <int> GroupCandidatesHighestPoints(List <int> highestPointIndexes, float groupingMaxDistance) { var groupedPoints = new List <int>(); var pointDepths = new Dictionary <int, float>(); for (int i = 0; i < highestPointIndexes.Count; i++) { if (!pointDepths.ContainsKey(highestPointIndexes[i])) { pointDepths.Add(highestPointIndexes[i], GlobVar.SubtractedFilteredPointCloud[highestPointIndexes[i]].Z); } } var sortedPointDepths = pointDepths.OrderBy(kvp => kvp.Value); foreach (var sortedPointDepth in sortedPointDepths) { int currentIndex = sortedPointDepth.Key; bool add = true; foreach (var groupedPoint in groupedPoints) { if (GlobUtils.GetEuclideanDistanceXYPlane(groupedPoint, currentIndex) < groupingMaxDistance) { add = false; } } if (add) { groupedPoints.Add(sortedPointDepth.Key); } } return(groupedPoints); }
/// <summary> /// Slides a window with a subwindow with sizes specified by the input parameters over the frame. The average depth in the windows are compared and if above a certain threshold considered to be head candidates. /// </summary> private List <int> SlideWindow(int outerWindowWidth, int innerWindowWidth, float windowDifferenceThreshold) { var candidates = new List <int>(); int marginInnerWindow = (outerWindowWidth - innerWindowWidth) / 2; int innerWindowArea = innerWindowWidth * innerWindowWidth; for (int i = 0; i < GlobVar.ScaledFrameHeight - innerWindowWidth; i += 2) { for (int j = 0; j < GlobVar.ScaledFrameWidth - innerWindowWidth; j += 2) { int outerWindowHeight = outerWindowWidth; int outerWindowTempWidth = outerWindowWidth; int iOuter = i - marginInnerWindow; int jOuter = j - marginInnerWindow; if (iOuter < 0) { outerWindowHeight += iOuter; iOuter = 0; } if (jOuter < 0) { outerWindowTempWidth += jOuter; jOuter = 0; } if (iOuter + outerWindowWidth > GlobVar.ScaledFrameHeight - 1) { outerWindowHeight += iOuter + outerWindowWidth - GlobVar.ScaledFrameHeight - 1; iOuter = GlobVar.ScaledFrameHeight - 1 - outerWindowHeight; } if (jOuter + outerWindowWidth > GlobVar.ScaledFrameWidth - 1) { outerWindowTempWidth += jOuter + outerWindowWidth - GlobVar.ScaledFrameWidth - 1; jOuter = GlobVar.ScaledFrameWidth - 1 - outerWindowTempWidth; } int innerIndexA = GlobUtils.GetIndex(j, i); int innerIndexB = GlobUtils.GetIndex(j + innerWindowWidth, i); int innerIndexC = GlobUtils.GetIndex(j, i + innerWindowWidth); int innerIndexD = GlobUtils.GetIndex(j + innerWindowWidth, i + innerWindowWidth); int outerIndexA = GlobUtils.GetIndex(jOuter, iOuter); int outerIndexB = GlobUtils.GetIndex(jOuter + outerWindowTempWidth, iOuter); int outerIndexC = GlobUtils.GetIndex(jOuter, iOuter + outerWindowHeight); int outerIndexD = GlobUtils.GetIndex(jOuter + outerWindowTempWidth, iOuter + outerWindowHeight); int outerWindowArea = GlobUtils.CalculateRectangleAreaFromIndexes(outerIndexA, outerIndexB, outerIndexC); float sumInnerWindow = (_integralImage[innerIndexA] + _integralImage[innerIndexD] - _integralImage[innerIndexB] - _integralImage[innerIndexC]); float sumOuterWindow = (_integralImage[outerIndexA] + _integralImage[outerIndexD] - _integralImage[outerIndexB] - _integralImage[outerIndexC]); float averageInnerWindow = sumInnerWindow / innerWindowArea; if (averageInnerWindow > GlobVar.MaxSensingDepth - Thresholds.HaarDetectionHeightHeadMin) { continue; } float averageOuterWindow = (sumOuterWindow - sumInnerWindow) / (outerWindowArea - innerWindowArea); if ((averageOuterWindow - averageInnerWindow) > windowDifferenceThreshold) { int candidate = GlobUtils.GetHighestValidPointIndexInRectangle(innerIndexB, innerIndexC); candidates.Add(candidate); if (Logger.ShowHaarInnerRects) { GraphicsUtils.DrawRectangle(new IndexRectangle(innerIndexA, innerIndexB, innerIndexC)); } if (Logger.ShowHaarOuterRects) { GraphicsUtils.DrawRectangle(new IndexRectangle(outerIndexA, outerIndexB, outerIndexC)); } } } } return(candidates); }
public static void DrawPoint(int i) { DrawPoint(GlobUtils.GetPoint(i)); }