def unpack_plane(mesh_, faces_idx): faces = mesh_.faces[faces_idx] area = sum(mesh_.area_faces[faces_idx]) normals = mesh_.face_normals[faces_idx] vertices = mesh_.vertices[faces] # if area > 0.4: # mesh_.visual.face_colors[faces_idx] = trimesh.visual.random_color() #TODO: use ransac find plane centroids = np.array([(fv[0] + fv[1] + fv[2]) / 3 for fv in vertices]) try: plane = pyrsc.Plane() eq, inliers = plane.fit(np.array(centroids), thresh=0.01, minPoints=100, maxIteration=100) normal = eq[:3] except: normal = np.average(normals, axis=0) points_idx = np.unique(faces) points = mesh_.vertices[points_idx] new_plane = Plane() new_plane.set_attribute('faces_idx', faces_idx) new_plane.set_attribute('faces', faces) new_plane.set_attribute('area', area) new_plane.set_attribute('normals', normals) new_plane.set_attribute('normal', normal) new_plane.set_attribute('vertices', vertices) new_plane.set_attribute('points_idx', points_idx) new_plane.set_attribute('points', points) return new_plane
def ransac_find_planes(pcd): planes = [] max_x = np.amax(pcd[:, 0]) min_x = np.amin(pcd[:, 0]) max_y = np.amax(pcd[:, 1]) min_y = np.amin(pcd[:, 1]) max_z = np.amax(pcd[:, 2]) min_z = np.amin(pcd[:, 2]) # xyz = pcd.copy() xyz_split = [pcd.copy()] for xyz in xyz_split: iters = 0 while (iters < 25): plane = pyrsc.Plane() eq, inliers = plane.fit(xyz, thresh=0.01, minPoints=10000, maxIteration=100) if (inliers.shape[0] > 10000): planes.append(np.array([eq, xyz[inliers]], dtype=object)) xyz = np.delete(xyz, inliers, axis=0) print(eq) iters = 0 else: iters += 1 return np.array(planes, dtype=object)
def inrange_segment_plane (pcd): # Remove points out of range of 80 meters arr = np.asarray(pcd.points) r = np.sqrt(np.square(arr[:,0])+np.square(arr[:,1])) inrange = np.where(r < 80) pcd_inrange = pcd.select_by_index(inrange[0], invert=False) arr_inrange = np.asarray(pcd_inrange.points) # Segment plane with RANSAC algorithm #segmplane = dfpc0.segment_plane(distance_threshold=0.5, ransac_n=3, num_iterations=120) plane1 = pyransac3d.Plane() best_eq, best_inliers = plane1.fit(arr_inrange, thresh=0.25, minPoints=1, maxIteration=100) #inliers_result = pd.DataFrame(segmplane[1]) # Convert list to numpy array #inliers_result_arr = np.asarray(inliers_result) #inliers_result_arr.astype(np.uint32) # Remove plane pcd_out = pcd_inrange.select_by_index(best_inliers, invert=True) #pcd_plane = dfpc0.select_by_index(inliers_result_arr, invert=False) # Visualize planes #o3d.visualization.draw_geometries([pcd_out]) #o3d.visualization.draw_geometries([pcd_plane]) # Clustering with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm: labels = np.array(pcd_out.cluster_dbscan(eps=0.6, min_points=25, print_progress=False)) #eps: distance to neighbor in cluster #min_points: min number of points required to form a cluster max_label = labels.max() print(f"point cloud has {max_label + 1} clusters") colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1)) colors[labels < 0] = 0 pcd_out.colors = o3d.utility.Vector3dVector(colors[:, :3]) #visualization # o3d.visualization.draw_geometries([pcd_out]) print("pcd_out") print(pcd_out) labelst = labels.transpose() print("labelst") print(labelst) return pcd_out
def ransac_fit_plane(pcd): planes = [] for xyz in pcd: plane = pyrsc.Plane() try: eq, inliers = plane.fit(xyz, thresh=0.01, minPoints=10000, maxIteration=100) print(eq, inliers) planes.append(np.array([eq, xyz[inliers]], dtype=object)) except ValueError: print(f'Unlikely a plane due to a size of {len(xyz)} points.') # np.save('surfaces', np.array(planes, dtype=object)) # print(f'Saved') # plot_layout2(pcd, planes) return planes
import sys import numpy as np import open3d as o3d sys.path.append(".") import pyransac3d as pyrsc # Load saved point cloud and visualize it pcd_load = o3d.io.read_point_cloud("tests/dataset/caixa.ply") # o3d.visualization.draw_geometries([pcd_load]) points = np.asarray(pcd_load.points) plano1 = pyrsc.Plane() best_eq, best_inliers = plano1.fit(points, 0.01) plane = pcd_load.select_by_index(best_inliers).paint_uniform_color([1, 0, 0]) obb = plane.get_oriented_bounding_box() obb2 = plane.get_axis_aligned_bounding_box() obb.color = [0, 0, 1] obb2.color = [0, 1, 0] not_plane = pcd_load.select_by_index(best_inliers, invert=True) o3d.visualization.draw_geometries([not_plane, plane, obb, obb2])
# VectUb = VectB/np.linalg.norm(VectB) # W = np.cross(VectUa,VectUb) # # D=np.sum(np.multiply(W, losowy_punkt_klaster1_3)) #obliczenie D # np.subtract(losowy_punkt_klaster1_1,losowy_punkt_klaster1_3) # W=list(W) # for temp in range(len(cluster1_x)-1): #stworzenie listy W do obliczania dystaunsu # W.append(W) # # distance = (np.add((np.multiply(coord,W)),D)) # # print(distance) ###################### cloud1 = np.array(list(zip(cluster1_x, cluster1_y, cluster1_z))) cloud2 = np.array(list(zip(cluster2_x, cluster1_y, cluster2_z))) cloud3 = np.array(list(zip(cluster3_x, cluster3_y, cluster3_z))) plane1 = pyransac3d.Plane() plane2 = pyransac3d.Plane() plane3 = pyransac3d.Plane() best_eq1, best_inliners1 = plane1.fit(cloud1, thresh=0.01, minPoints=100, maxIteration=1000) best_eq2, best_inliners2 = plane1.fit(cloud2, thresh=0.01, minPoints=100, maxIteration=1000) best_eq3, best_inliners3 = plane1.fit(cloud3, thresh=0.01, minPoints=100, maxIteration=1000) print(f'Best equasion for plane 1 Ax+Bx+Cx+D=0:{best_eq1}')
from csv import reader import numpy as np import pyransac3d as pyrsc def read_csv(name, nl="\n", dl=","): cloud = [] with open(name, newline=nl) as csvfile: CsvReader = reader(csvfile, delimiter=dl) for xx, yy, zz in CsvReader: cloud.append([float(xx), float(yy), float(zz)]) return cloud cloud = np.array(read_csv("cloud_c.xyz")) plane = pyrsc.Plane() best_eq, best_inliers = plane.fit(cloud, thresh=0.02, minPoints=100, maxIteration=1000) print(f'best equation Ax+By+Cz+D = {best_eq}') print(f'best inliers: {best_inliers}')
# ######################################################## # ######################################################## X = np.array(list_of_read_points_from_csv) # ######################################################## # ######################################################## # ransac_algorithm(X, 10, 0.1) # ######################################################## # ######################################################## k_mean_lib(X) # ######################################################## # ######################################################## # points = load_points(.) # Load your point cloud as a numpy array (N, 3) points = X # Example 1 - Planar RANSAC plane1 = pyrsc.Plane() best_eq, best_inliers = plane1.fit(points, 0.01) # Results in the plane equation Ax+By+Cz+D: [1, 0.5, 2, 0] # Example 2 - Spherical RANSAC # Loading a noisy sphere's point cloud with r = 5 centered in 0 we can use the following code: # points = load_points(.) # Load your point cloud as a numpy array (N, 3) points = X sph = pyrsc.Sphere() center, radius, inliers = sph.fit(points, thresh=0.4) # center: [0.010462385575072288, -0.2855090643954039, 0.02867848979091283] # radius: 5.085218633039647 # ######################################################## # ########################################################
from csv import reader import numpy import pyransac3d def read_csv(name,nl="\n",dl=","): cloud=[] with open(name,newline=nl) as csvfile: csvreader=reader(csvfile,delimiter=dl) for xx, yy, zz in csvreader: cloud.append([float(xx), float(yy), float(zz)]) return cloud if __name__=="__main__": cloud=numpy.array(read_csv("LidarData.xyz")) plane=pyransac3d.Plane() best_eq, best_inliers=plane.fit(cloud, thresh=0.01, minPoints=100, maxIteration=1000) print(f'best euation Ax+By+Cz+D:{best_eq}') print(f'best inliers:{best_inliers}')