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