├── BPD.py ├── Circle.py ├── ConcaveHull.py ├── README.md ├── after1.png ├── after2(applied round).png ├── before.png ├── main.py ├── plane_extractor.py ├── points.txt ├── polar.py └── remove_wall.py /BPD.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import Circle as C 3 | import open3d as o3d 4 | import polar 5 | from sklearn.neighbors import NearestNeighbors 6 | from itertools import combinations 7 | 8 | def cal_boundary(points, k=30, save_filename=None) : 9 | neighbors = NearestNeighbors(n_neighbors=k).fit(points) 10 | distances, indices = neighbors.kneighbors(points) 11 | 12 | max_b = 0 13 | min_b = 1000000000 14 | 15 | boundary = [] 16 | for i in range(points.shape[0]) : 17 | is_boundary = False 18 | p_neighbor, p_distance = indices[i], np.round(distances[i], 5) 19 | neighbor_points = points[p_neighbor[1:]] 20 | 21 | p_mean = np.mean(p_distance) 22 | p_std = np.std(p_distance) 23 | local_resol = round(p_mean + 2 * p_std, 5) 24 | 25 | if local_resol > max_b : 26 | max_b = local_resol 27 | if min_b > local_resol : 28 | min_b = local_resol 29 | 30 | pairs = list(combinations(p_neighbor[1:], 2)) 31 | print(i) 32 | for j in range(len(pairs)) : 33 | count = 0 34 | p1 = points[i] 35 | p2 = points[pairs[j][0]] 36 | p3 = points[pairs[j][1]] 37 | c = C.Circle(p1, p2, p3) 38 | if c.radius == None : 39 | continue 40 | 41 | if c.radius >= local_resol : 42 | cn_distance = np.linalg.norm((neighbor_points - c.center), axis=1) 43 | cn_distance = np.round(cn_distance, 5) 44 | 45 | for k in range(len(cn_distance)) : 46 | if cn_distance[k] <= c.radius : 47 | count += 1 48 | 49 | if count > 3 : 50 | break 51 | 52 | if count == 3 : 53 | boundary.append(points[i]) 54 | is_boundary = True 55 | break 56 | 57 | if not is_boundary : 58 | pol = polar.Polar(np.array(points[i]), neighbor_points, normalize=True) 59 | 60 | 61 | 62 | print(f'len : {len(boundary)}') 63 | print(f'{min_b}') 64 | print(f'{max_b}') 65 | pc = o3d.geometry.PointCloud() 66 | pc.points = o3d.utility.Vector3dVector(np.pad(np.array(boundary), (0, 1), 'constant', constant_values=0)) 67 | 68 | o3d.visualization.draw_geometries([pc]) 69 | points = np.array(pc.points) 70 | if save_filename != None : 71 | 72 | np.savetxt(save_filename, points[:, :2]) 73 | 74 | return points 75 | 76 | 77 | -------------------------------------------------------------------------------- /Circle.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | class Circle : 5 | def __init__(self, p1, p2, p3) : 6 | v1 = np.dot(p2, p2.transpose()) 7 | v2 = (np.dot(p1, p1.transpose()) - v1) / 2 8 | v3 = (v1 - (p3[0]**2) - (p3[1]**2)) / 2 9 | det = (p1[0] - p2[0]) * (p2[1] - p3[1]) - (p2[0] - p3[0]) * (p1[1] - p2[1]) 10 | 11 | if abs(det) < 1.0e-10: 12 | self.radius = None 13 | self.center = None 14 | return 15 | 16 | # Center of circle 17 | cx = round((v2 * (p2[1] - p3[1]) - v3 * (p1[1] - p2[1])) / det, 5) 18 | cy = round(((p1[0] - p2[0]) * v3 - (p2[0] - p3[0]) * v2) / det, 5) 19 | 20 | self.center = np.array([cx,cy]) 21 | self.radius = round(np.linalg.norm(p1-self.center), 5) 22 | 23 | def print(self) : 24 | print(f'R : {self.radius} C : {self.center}') 25 | 26 | def to_array(self) : 27 | arr = [[self.radius * math.sin(i), self.radius * math.cos(i)] for i in range(0, 360, 10)] 28 | return np.array(arr) 29 | 30 | -------------------------------------------------------------------------------- /ConcaveHull.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import sklearn.neighbors as NearestNeighbors 4 | 5 | def is_same_point(p1, p2) : 6 | return (p1[0] == p2[0]) and (p1[1] == p2[1]) 7 | 8 | #sort neightbors in descending order of right-hand term 9 | def sort_by_angle(center, neighbors, prevAngle) : 10 | pass 11 | 12 | #cal angle between two point 13 | def angle(p1, p2) : 14 | pass 15 | 16 | #check intersect 17 | def is_intersect(...) : 18 | pass 19 | 20 | def concavehull(input_points, k=3) : 21 | k = max(k,3) 22 | points = np.unique(input_points, axis=0) 23 | 24 | points_length = points.shape[0] 25 | if points_length < 3 : 26 | print("points must be 3>") 27 | return None 28 | if points_length == 3 : 29 | return points 30 | 31 | k = min(k, points_length) 32 | 33 | knn = NearestNeighbors(n_neighbors=k).fit(points) 34 | distances, indices = knn.kneighbors(points) 35 | 36 | #find minYpoint 37 | min_y_ind = np.argmin(points[:, 1]) 38 | start_point = points(min_y_ind) 39 | points = np.delete(points, min_y_ind) 40 | current_point = start_point 41 | 42 | hull = [current_point] 43 | prev_angle = 0 44 | step = 2 45 | 46 | while (not is_same_point(current_point, start_point) or step == 2) and points.shape[0] > 0: 47 | if step == 5 : 48 | points.append(start_point) 49 | 50 | #current point에 대한 neighbor 구하기 51 | neighbors = None 52 | c_point = sort_by_angle(current_point, neighbors, prev_angle) 53 | c_point_length = c_point.shape[0] 54 | its = True 55 | i = 0 56 | 57 | while its and i < c_point_length : 58 | i += 1 59 | if is_same_point(c_point[i], start_point) : 60 | last_point = 1 61 | else 62 | last_point = 0 63 | j = 2 64 | 65 | its = False 66 | hull_length = len(hull) 67 | while not its and j < (hull_length - last_point) : 68 | #intersection 체크 69 | its = is_intersect() 70 | j += 1 71 | 72 | if its : 73 | return concavehull(points, k+1) 74 | 75 | current_point = c_point[i] 76 | hull.append(current_point) 77 | prev_angle = angle(hull[step], hull[step-1]) 78 | step += 1 79 | 80 | #check all inside 81 | if is_all_inside(input_points, hull) : 82 | return hull 83 | else : 84 | return concavehull4(input_points, k+1) 85 | 86 | 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BoundaryDetection-Algorithm-for-Point-Cloud-Data 2 | Implement 2D point cloud boundary detection From "Novel algorithms for 3D surface point cloud boundary detection and edge reconstruction" using Python 3 | 4 | "Carmelo Mineo, Stephen Gareth Pierce, Rahul Summan, Novel algorithms for 3D surface point cloud boundary detection and edge reconstruction, Journal of Computational Design and Engineering, Volume 6, Issue 1, January 2019, Pages 81–91, https://doi.org/10.1016/j.jcde.2018.02.001" 5 | -------------------------------------------------------------------------------- /after1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qutom/BoundaryDetection-Algorithm-for-Point-Cloud-Data/c7bf6e9b8db81afb086b2f1a73576f2377978cf8/after1.png -------------------------------------------------------------------------------- /after2(applied round).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qutom/BoundaryDetection-Algorithm-for-Point-Cloud-Data/c7bf6e9b8db81afb086b2f1a73576f2377978cf8/after2(applied round).png -------------------------------------------------------------------------------- /before.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Qutom/BoundaryDetection-Algorithm-for-Point-Cloud-Data/c7bf6e9b8db81afb086b2f1a73576f2377978cf8/before.png -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import plane_extractor 2 | import BPD 3 | import numpy as np 4 | import remove_wall 5 | import open3d as o3d 6 | 7 | def display_outlier(points, ind): 8 | inlier = points.select_by_index(ind) 9 | outlier = points.select_by_index(ind, invert=True) 10 | inlier.paint_uniform_color([0.7, 0.7, 0.7]) 11 | outlier.paint_uniform_color([1, 0, 0]) 12 | o3d.visualization.draw_geometries([inlier, outlier]) 13 | 14 | pc = o3d.io.read_point_cloud("cropped_1.ply") 15 | #o3d.visualization.draw_geometries([pc]) 16 | 17 | plane, high_z, min_z = plane_extractor.extract_plane(pc) 18 | boundary = BPD.cal_boundary(plane, save_filename="ceil_boundary.txt") 19 | #boundary = np.loadtxt("ceil_boundary.txt") 20 | points = np.array(pc.points) 21 | high_z = float(np.max(points[:, 2])) 22 | min_z = float(np.min(points[:, 2])) 23 | 24 | props = remove_wall.remove_wall(points, boundary, method="cylinder", radius=0.07, min_z=min_z, high_z=high_z) 25 | 26 | o3d.visualization.draw_geometries([props]) 27 | 28 | -------------------------------------------------------------------------------- /plane_extractor.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | 4 | 5 | def display_outlier(points, ind): 6 | inlier = points.select_by_index(ind) 7 | outlier = points.select_by_index(ind, invert=True) 8 | inlier.paint_uniform_color([0.7, 0.7, 0.7]) 9 | outlier.paint_uniform_color([1, 0, 0]) 10 | o3d.visualization.draw_geometries([inlier, outlier]) 11 | 12 | 13 | def extract_plane(pointcloud, show_progress=False, voxel_size=0.1, nb_neighbors=30, std_ratio=2.0, z_ratio=0.25, 14 | save_filename=None): 15 | if show_progress: 16 | o3d.visualization.draw_geometries([pointcloud]) 17 | 18 | # remove outlier 19 | pc_down = pointcloud.voxel_down_sample(voxel_size=voxel_size) 20 | cl, ind = pc_down.remove_statistical_outlier(nb_neighbors=nb_neighbors, std_ratio=std_ratio) 21 | if show_progress: 22 | display_outlier(pc_down, ind) 23 | 24 | pc_down = pc_down.select_by_index(ind) 25 | 26 | # extract ceil 27 | high_z = -10000 28 | min_z = 1000000 29 | high_ind = 0 30 | low_ind = 0 31 | points = pc_down.points 32 | for i in range(len(points)): 33 | if points[i][2] > high_z: 34 | high_z = points[i][2] 35 | high_ind = i 36 | 37 | if points[i][2] < min_z: 38 | min_z = points[i][2] 39 | low_ind = i 40 | 41 | if show_progress: 42 | print(f'{high_z} , {min_z}') 43 | high_point = pc_down.select_by_index([high_ind]) 44 | low_point = pc_down.select_by_index([low_ind]) 45 | temp = pc_down.select_by_index([high_ind, low_ind], invert=True) 46 | temp.paint_uniform_color([0.7, 0.7, 0.7]) 47 | high_point.paint_uniform_color([1, 0, 0]) 48 | low_point.paint_uniform_color([0, 1, 0]) 49 | 50 | o3d.visualization.draw_geometries([low_point, high_point, temp]) 51 | 52 | # Parameter 53 | z_threshold = abs(high_z - min_z) * z_ratio 54 | inliers = [] 55 | for i in range(len(points)) : 56 | if points[i][2] >= z_threshold : 57 | inliers.append(i) 58 | 59 | # remove outlier in ceil 60 | segments = pc_down.select_by_index(inliers) 61 | if show_progress : 62 | o3d.visualization.draw_geometries([segments]) 63 | 64 | cl, ind = segments.remove_statistical_outlier(nb_neighbors=nb_neighbors, std_ratio=std_ratio) 65 | if show_progress: 66 | display_outlier(segments, ind) 67 | segments = segments.select_by_index(ind) 68 | 69 | # project to highest Z value 70 | points = np.array(segments.points) 71 | points[:, 2] = high_z 72 | segments.points = o3d.utility.Vector3dVector(points) 73 | 74 | if show_progress: 75 | o3d.visualization.draw_geometries([segments]) 76 | 77 | points = np.array(segments.points) 78 | 79 | if save_filename != None: 80 | np.savetxt(save_filename, points[:, :2]) 81 | 82 | return points[:, :2], high_z, min_z 83 | -------------------------------------------------------------------------------- /polar.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import random 4 | import Circle as C 5 | import open3d as o3d 6 | import alphashape 7 | from sklearn.neighbors import NearestNeighbors 8 | from itertools import combinations 9 | 10 | class Polar : 11 | def __init__(self, center, points, normalize=False): 12 | self.r, self.theta = self.cart2pol(center, points) 13 | self.min_theta = np.min(self.theta) 14 | self.max_theta = np.max(self.theta) 15 | self.min_r = np.min(self.r) 16 | self.max_r = np.max(self.r) 17 | 18 | if normalize : 19 | self.polar_normalize() 20 | 21 | self.bis = np.min(self.theta) + np.max(self.theta) / 2 22 | 23 | def cart2pol(self, center, points, show=False): 24 | points -= center 25 | 26 | x = points[:, 0] 27 | y = points[:, 1] 28 | 29 | r = np.sqrt(x ** 2 + y ** 2) 30 | theta = np.arctan2(y, x) 31 | if show : 32 | print(r) 33 | print(theta) 34 | return r, theta 35 | 36 | def polar_normalize(self): 37 | 38 | self.r = (self.r - self.min_r) / (self.max_r - self.min_r) 39 | self.theta = (self.theta - self.min_theta) / (self.max_theta - self.min_theta) 40 | 41 | def plot(self): 42 | fig, ax = plt.subplots(subplot_kw={'projection': 'polar'}) 43 | ax.scatter(self.theta, self.r) 44 | 45 | max_r = np.max(self.r) 46 | ax.set_rmax(max_r * 1.5) 47 | ax.grid(True) 48 | 49 | plt.show() 50 | 51 | def check(self): 52 | #set starting point 53 | theta_bis = np.max(self.max_theta) - np.min(self.min_theta) 54 | chr_param = self.r + np.abs(self.theta - self.theta_bis) 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /remove_wall.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | 4 | """ 5 | @Params : points : numpy array 6 | boundary : numpy array 7 | raidus : float 8 | min_z : float 9 | high_z : float 10 | @Return : pointcloud data 11 | """ 12 | def remove_cylinder(points, boundary, radius, min_z, high_z) : 13 | pc = o3d.geometry.PointCloud() 14 | pc.points = o3d.utility.Vector3dVector(points) 15 | 16 | for i in range(boundary.shape[0]) : 17 | z = np.arange(min_z, high_z, radius * 0.7) 18 | pillar_points = np.tile(boundary[i], (z.shape[0], 1)) 19 | 20 | pillar_points = np.hstack((pillar_points, z.reshape(z.shape[0], 1))) 21 | pillar_pc = o3d.geometry.PointCloud() 22 | pillar_pc.points = o3d.utility.Vector3dVector(pillar_points) 23 | 24 | dists = np.asarray(pc.compute_point_cloud_distance(pillar_pc)) 25 | ind = np.where(dists > radius)[0] 26 | pc = pc.select_by_index(ind) 27 | print(i) 28 | return pc 29 | 30 | """ 31 | @Params : points : numpy array 32 | boundary : numpy array 33 | @Return : numpy array 34 | """ 35 | def remove_wall(points, boundary, method="edge", radius=0.5, box_size=0.5, min_z=None, high_z=None) : 36 | if method == "edge" : 37 | #use edge algorithm 38 | pass 39 | if method == "box" : 40 | #use box algorithm 41 | pass 42 | if method == "cylinder" : 43 | result = remove_cylinder(points, boundary, radius=radius, min_z=min_z, high_z=high_z) 44 | 45 | return result --------------------------------------------------------------------------------