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