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