private bool Helper_FindNeighbours(ref PointCloudVertices pointsSource, ref PointCloudVertices pointsTarget, KDTreeVertex kdTree, float angleThreshold)
{
if (!ICPSettings.FixedTestPoints)
{
if (ICPSettings.KDTreeMode == KDTreeMode.Stark)
{
pointsTarget = kdTree.FindNearest_Stark(pointsSource, pointsTarget);
}
else if (ICPSettings.KDTreeMode == KDTreeMode.Rednaxela)
{
pointsTarget = kdTree.FindNearest_Rednaxela_Parallel(ref pointsSource, pointsTarget, angleThreshold);
//pointsTarget = kdTree.FindNearest_Rednaxela(ref pointsSource, pointsTarget, angleThreshold);
}
else if (ICPSettings.KDTreeMode == KDTreeMode.Rednaxela_ExcludePoints)
{
pointsTarget = kdTree.FindNearest_Rednaxela_ExcludePoints(pointsSource, pointsTarget, false);
}
else if (ICPSettings.KDTreeMode == KDTreeMode.BruteForce)
{
pointsTarget = kdTree.FindNearest_BruteForce(pointsSource, pointsTarget);
}
}
else
{
//adjust number of points - for the case if there are outliers
int min = pointsSource.Count;
if (pointsTarget.Count < min)
{
min = pointsTarget.Count;
pointsSource.RemoveRange(pointsTarget.Count, pointsSource.Count - min);
}
else
{
pointsTarget.RemoveRange(pointsSource.Count, pointsTarget.Count - min);
}
}
return(true);
}