/// <summary>
/// returns a list of points of tetrahedrons in a given point cloud
/// </summary>
/// <param name="pointCloud">input point cloud</param>
/// <returns>list of tetrahedrons</returns>
public static List <List <Point> > calculateDelaunayTrianglePoints(PointCloud pointCloud)
{
List <List <Point> > retList = new List <List <Point> >();
try
{
//create vertex list
HashSet <Vertex> vertices = new HashSet <Vertex>();
foreach (Point p in pointCloud.pointcloud_hs)
{
vertices.Add(new Vertex(p.point.X, p.point.Y, p.point.Z));
}
//calculate tetrahedrons
var tetrahedrons = Triangulation.CreateDelaunay <Vertex, Tetrahedron>(vertices).Cells;
foreach (var c in tetrahedrons)
{
List <Point> pL = new List <Point>();
for (int i = 0; i < 4; i++)
{
Point p = new Point(new ANX.Framework.Vector3((float)c.Vertices[i].Position[0], (float)c.Vertices[i].Position[1], (float)c.Vertices[i].Position[2]));
pL.Add(p);
}
retList.Add(pL);
}
}
catch (Exception) { throw; }
return(retList);
}
/// <summary>
/// calculates the best fit plane model using RANSAC
/// </summary>
/// <param name="pPointSet">the point set to get the plane from</param>
/// <param name="pIterationThreshold">the amount of iterations to do</param>
/// <param name="pPlanedistanceThreshold"></param>
/// <returns></returns>
static List <PlaneModel> doPlanarModelSegmentation(PointCloud pPointSet, int pIterationThreshold, float pPlanedistanceThreshold, int pNumberOfPlanes, float pPlaneComparisonVariance, CancellationToken pTaskToken)
{
//abort if requested
pTaskToken.ThrowIfCancellationRequested();
//create bag of models
List <TModel> models = new List <TModel>();
Object tLock = new Object();
//iterate using multithreading
Parallel.For(0, pIterationThreshold, t =>
{
//create new model to watch
TModel currentModel = new TModel();
int inliers = 0;
//select 3 random points to generate plane from
Point[] randomPoints = new Point[3];
Random rand = new Random(t * 1024);
for (int i = 0; i < 3; i++)
{
randomPoints[i] = pPointSet.pointcloud_hs.ElementAt(rand.Next(pPointSet.count));
}
//create Plane
currentModel.plane = new PlaneModel(new ANX.Framework.Vector3(randomPoints[0].point.X, randomPoints[0].point.Y, randomPoints[0].point.Z),
new ANX.Framework.Vector3(randomPoints[1].point.X, randomPoints[1].point.Y, randomPoints[1].point.Z),
new ANX.Framework.Vector3(randomPoints[2].point.X, randomPoints[2].point.Y, randomPoints[2].point.Z));
//iterate through set and count inliers
foreach (Point p in pPointSet.pointcloud_hs)
{
//check for abortion
pTaskToken.ThrowIfCancellationRequested();
//check distance, if smaller than threshold, increase count
float dis = PointCloud.calculateDistancePointToPlane(p, currentModel.plane.anxPlane);
if (dis < pPlanedistanceThreshold)
{
inliers++;
}
}
//add to models
currentModel.plane.inliers = inliers;
currentModel.inliers = inliers;
lock (tLock)
models.Add(currentModel);
});
//sort by inliers
var mod = models.OrderByDescending(t => t.inliers);
//check for abortion
pTaskToken.ThrowIfCancellationRequested();
//check list for existing plane models
List <PlaneModel> resList = new List <PlaneModel>();
foreach (TModel pl in mod)
{
bool found = false;
foreach (PlaneModel pm in resList)
{
if (PlaneModel.comparePlanes(pm, pl.plane, pPlaneComparisonVariance))
{
found = true; break;
}
}
//if not found, add
if (!found)
{
//check for point validity
if (!PlaneModel.comparePlanePoints(pl.plane, pPlanedistanceThreshold * 20))
{
resList.Add(pl.plane);
}
}
//if list big enough, break
if (resList.Count >= pNumberOfPlanes)
{
break;
}
}
return(resList);
}
/// <summary>
/// calculates an euclidean cluster extraction from the point cloud based on a euclidean distance. returns a list of point clouds
/// </summary>
/// <param name="pInputCloud">the combined point cloud</param>
/// <param name="pConfig">the config object for the algorithms</param>
/// <returns>a list of point clouds</returns>
public static List <PointCloud> calculateEuclideanClusterExtraction(PointCloud pInputCloud, float pEuclideanExtractionRadius, CancellationToken pToken)
{
//update status
Log.LogManager.updateAlgorithmStatus("Euclidean Cluster Extraction");
Log.LogManager.writeLogDebug("[EuclideanClusterExtraction] Extraction Radius: " + pEuclideanExtractionRadius);
//creates the end list to be returned
List <PointCloud> clusters = new List <PointCloud>();
foreach (Point p in pInputCloud.pointcloud_hs)
{
pToken.ThrowIfCancellationRequested();
//checks if point has been processed already, if yes, skip
if (p.processed)
{
continue;
}
//create new queue
Queue <Point> Q = new Queue <Point>();
//add point to queue
Q.Enqueue(p);
//result values
HashSet <Point> resultCloud = new HashSet <Point>();
PointCloud resultCluster = new PointCloud(resultCloud);
//while queue has points, check for neighbours and add them to queue as well, do as long there are neighbours
while (Q.Count > 0)
{
pToken.ThrowIfCancellationRequested();
//remove point from queue and add to current cluster, point has been processed by doing that
Point p2 = Q.Dequeue();
if (p2.processed)
{
continue;
}
resultCloud.Add(p2);
p2.processed = true;
//check all neighbour points, add them to queue if they havnt been processed yet
double[] tp = { p2.point.X, p2.point.Y, p2.point.Z };
NearestNeighbour <Point> nb = lookForNeighbours(pInputCloud.pointcloud_kd, tp, pEuclideanExtractionRadius);
while (nb.MoveNext())
{
if (!nb.Current.processed)
{
Q.Enqueue(nb.Current);
}
}
}
clusters.Add(resultCluster);
}
//set all points as unprocessed, so they can be used for other algorithms
Parallel.ForEach(clusters, pointcloud => Parallel.ForEach(pointcloud.pointcloud_hs, point => point.processed = false));
//updates the status
Log.LogManager.updateAlgorithmStatus("Done");
//return
return(clusters);
}
