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