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