def main(): letter_map = read_alphabet() # letters = ['P', 'O', 'L', 'Y', 'L', 'I', 'D', 'A', 'R'] points = stitch_letters(letter_map) # plt.scatter(points[:, 0], points[:, 1], s=0.2) # plt.show() print(int(points.shape[0] / 2)) idx = np.random.randint(0, points.shape[0], size=int(points.shape[0] / 2)) points = np.ascontiguousarray(points[idx, :]) lmax = 10.0 polylidar_kwargs = dict(alpha=0.0, lmax=lmax, min_triangles=5) # print(polylidar_kwargs) # Convert Points and make Polylidar points_mat = MatrixDouble(points) polylidar = Polylidar3D(**polylidar_kwargs) # Extracts planes and polygons, time t1 = time.perf_counter() mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.perf_counter() triangles = np.asarray(mesh.triangles) # print(triangles, triangles.shape) triangles = triangles.flatten() # print(triangles, triangles.shape) planes_np = planes # print(planes_np) print("Took {:.2f} milliseconds".format((t2 - t1) * 1000)) # Plot Data if points.shape[0] < 100000: fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1) ax.set_xticks([]) ax.set_yticks([]) plt.axis('equal') plt.axis('off') # plot points plot_points(points, ax) fig.savefig('assets/scratch/pl_logo_points.png', bbox_inches='tight', transparent=True) # plot all triangles plot_triangles(get_triangles_from_list(triangles, points), ax) # plot mesh triangles triangle_meshes = get_colored_planar_segments(planes_np, triangles, points) plot_triangle_meshes(triangle_meshes, ax) # plot polygons plot_polygons(polygons, points, ax, linewidth=4.0) plt.subplots_adjust(wspace=0.185, hspace=0.185, left=0.110, top=0.535, right=0.750, bottom=0.110) fig.savefig('assets/scratch/pl_logo.png', bbox_inches='tight', transparent=True) plt.show()
def create_meshes_cuda(opc, **kwargs): """Creates a mesh from a noisy organized point cloud Arguments: opc {ndarray} -- Must be MXNX3 Keyword Arguments: loops {int} -- How many loop iterations (default: {5}) _lambda {float} -- weighted iteration movement (default: {0.5}) Returns: [tuple(mesh, o3d_mesh)] -- polylidar mesh and o3d mesh reperesentation """ if opf is not None: smooth_opc, opc_normals, timings = laplacian_then_bilateral_opc_cuda(opc, **kwargs) tri_mesh, tri_map, time_elapsed_mesh = create_mesh_from_organized_point_cloud(smooth_opc, calc_normals=False) tri_norms = pick_valid_normals(tri_map, opc_normals) opc_normals_cp = MatrixDouble(tri_norms, copy=True) # copy here!!!!! # plot_triangle_normals(np.asarray(tri_mesh.triangle_normals), opc_normals) tri_mesh.set_triangle_normals(opc_normals_cp) # copy again here....sad else: tri_mesh, tri_map, time_elapsed_mesh = create_mesh_from_organized_point_cloud(opc, calc_normals=True) t1 = time.perf_counter() laplacian_filter_vertices(tri_mesh, iterations=kwargs['loops_laplacian'], _lambda=kwargs['_lambda']) t2 = time.perf_counter() bilateral_filter_normals(tri_mesh, iterations=kwargs['loops_bilateral'], sigma_length=kwargs['sigma_length'], sigma_angle=kwargs['sigma_angle']) t3 = time.perf_counter() t_laplacian = (t2 - t1) * 1000 t_bilateral = (t3 - t2) * 1000 timings = dict(t_laplacian=t_laplacian, t_bilateral=t_bilateral) timings = dict(**timings, t_mesh=time_elapsed_mesh) return tri_mesh, timings
def main(): points = load_csv('robust_1.csv') polylidar_kwargs = dict(alpha=0.0, lmax=1000.0, min_triangles=1) points_mat = MatrixDouble(points) polylidar = Polylidar3D(**polylidar_kwargs) # Extracts planes and polygons, time t1 = time.time() # pick large lmax to get the convex hull, no triangles filtered mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.time() print("Took {:.2f} milliseconds".format((t2 - t1) * 1000)) print( "If Robust Geometric Predicates is NOT activated, you should see a malformed convex hull" ) print( "See README.md to activate if desired. ~20% performance penalty when active." ) print("") # Convert to numpy format, no copy with np.asarray() triangles = np.asarray(mesh.triangles) fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1) # plot points ax.scatter(points[:, 0], points[:, 1], c='k') # plot all triangles plot_triangles(get_triangles_from_list(triangles, points), ax) # plot seperated planar triangular segments triangle_meshes = get_colored_planar_segments(planes, triangles, points) plot_triangle_meshes(triangle_meshes, ax) # plot polygons plot_polygons(polygons, points, ax) plt.axis('equal') plt.show()
def extract_planes_and_polygons_from_mesh(tri_mesh, avg_peaks, polylidar_kwargs=dict(alpha=0.0, lmax=0.1, min_triangles=2000, z_thresh=0.1, norm_thresh=0.95, norm_thresh_min=0.95, min_hole_vertices=50, task_threads=4), filter_polygons=True, pl_=None, optimized=False, postprocess=dict(filter=dict(hole_area=dict(min=0.025, max=100.0), hole_vertices=dict(min=6), plane_area=dict(min=0.05)), positive_buffer=0.00, negative_buffer=0.00, simplify=0.0)): if pl_ is not None: pl = pl_ else: pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks) t0 = time.perf_counter() if optimized: all_planes, all_polygons = pl.extract_planes_and_polygons_optimized(tri_mesh, avg_peaks_mat) else: all_planes, all_polygons = pl.extract_planes_and_polygons(tri_mesh, avg_peaks_mat) t1 = time.perf_counter() # tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) # planes_tri_set = [np.argwhere(np.asarray(tri_set) == i) for i in range(1, 2)] # o3d_mesh_painted = paint_planes(o3d_mesh, planes_tri_set) polylidar_time = (t1 - t0) * 1000 # logging.info("Polygon Extraction - Took (ms): %.2f", polylidar_time) all_planes_shapely = [] all_obstacles_shapely = [] time_filter = [] time_geometric_planes = [] # all_poly_lines = [] geometric_planes = [] # all_polygons = [[NORMAL_0_POLY_1, NORMAL_0_POLY_2], [NORMAL_1_POLY_1]] if filter_polygons: vertices = np.asarray(tri_mesh.vertices) for i in range(avg_peaks.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) # Rotating matrix the polygon polygons_for_normal = all_polygons[i] planes = all_planes[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: planes_shapely, obstacles_shapely, planes_indices, filter_time = filter_and_create_polygons( vertices, polygons_for_normal, rm=rm, postprocess=postprocess) t3 = time.perf_counter() geometric_planes_for_normal = [extract_geometric_plane(plane_poly[0], planes[plane_idx], tri_mesh, avg_peak) for ( plane_poly, plane_idx) in zip(planes_shapely, planes_indices)] geometric_planes.append(geometric_planes_for_normal) t4 = time.perf_counter() time_geometric_planes.append((t4 - t3) * 1000) all_planes_shapely.extend(planes_shapely) all_obstacles_shapely.extend(obstacles_shapely) time_filter.append(filter_time) # all_poly_lines.extend(poly_lines) timings = dict(t_polylidar_planepoly=polylidar_time, t_polylidar_filter=np.array(time_filter).mean(), t_geometric_planes=np.array(time_geometric_planes).sum()) # all_planes_shapely, all_obstacles_shapely, all_poly_lines, timings return all_planes_shapely, all_obstacles_shapely, geometric_planes, timings
def get_polygon(depth_image: np.ndarray, config, ll_objects, h, w, intrinsics, **kwargs): """Extract polygons from point cloud Arguments: points {ndarray} -- NX3 numpy array bonfig {dict} -- Configuration object h {int} -- height w {int} -- width Returns: tuple -- polygons, rotated downsample points, and rotation matrix """ # 1. Convert depth frame to downsampled organized point cloud # 2. Create a smooth mesh from the organized point cloud (OrganizedPointFilters) # 1. You can skip smoothing if desired and only rely upon Intel Realsense SDK # 3. Estimate dominate plane normals in scene (FastGA) # 4. Extract polygons from mesh using dominant plane normals (Polylidar3D) alg_timings = dict() # 1. Create OPC stride = config['mesh']['stride'] # point cloud generation parameters depth_scale = 1 / config['sensor_meta']['depth_scale'] depth_image = np.divide(depth_image, depth_scale, dtype=np.float32) points = extract_point_cloud_from_float_depth(MatrixFloat( depth_image), MatrixDouble(intrinsics), IDENTITY_MAT, stride=stride) new_shape = (int(depth_image.shape[0] / stride), int(depth_image.shape[1] / stride), 3) opc = np.asarray(points).reshape(new_shape) # organized point cloud (will have NaNs!) # print("OPC Shape: ", new_shape) # 2. Create Mesh and Smooth (all in one) # mesh, o3d_mesh, timings = create_meshes_cuda_with_o3d(opc, **config['mesh']['filter']) if config['mesh'].get('use_cuda'): mesh, timings = create_meshes_cuda(opc, **config['mesh']['filter']) else: mesh, timings = create_meshes(opc, **config['mesh']['filter']) alg_timings.update(timings) # 3. Estimate Dominate Plane Normals fga = config['fastga'] avg_peaks, _, _, _, timings = extract_all_dominant_plane_normals( mesh, ga_=ll_objects['ga'], ico_chart_=ll_objects['ico'], **fga) alg_timings.update(timings) # print(avg_peaks) # 4. Extract Polygons from mesh planes, obstacles, geometric_planes, timings = extract_planes_and_polygons_from_mesh(mesh, avg_peaks, pl_=ll_objects['pl'], filter_polygons=True, optimized=True, postprocess=config['polygon']['postprocess']) alg_timings.update(timings) # Uncomment to save raw data if desired # np.save('L515_Depth.npy', depth_image) # np.save('L515_OPC.npy', opc) # save_dict_to_json('L515_meta.json', dict(depth_scale=depth_scale, intrinsics=intrinsics.tolist(), # mesh=config['mesh'], fastga=config['fastga'], # polylidar=config['polylidar'], postprocess=config['polygon']['postprocess'])) # return planes, obstacles, geometric_planes, alg_timings, o3d_mesh return planes, obstacles, geometric_planes, alg_timings
def create_mesh_from_organized_point_cloud(pcd, rows=500, cols=500, stride=2): """Create Mesh from organized point cloud If an MXNX3 Point Cloud is passed, rows and cols is ignored (we know the row/col from shape) If an KX3 Point Cloud is passed, you must pass the row, cols, and stride that correspond to the point cloud Arguments: pcd {ndarray} -- Numpy array. Either a K X 3 (flattened) or MXNX3 Keyword Arguments: rows {int} -- Number of rows (default: {500}) cols {int} -- Number of columns (default: {500}) stride {int} -- Stride used in creating point cloud (default: {2}) Returns: tuple -- Polylidar Tri Mesh and O3D mesh """ pcd_ = pcd if pcd.ndim == 3: rows = pcd.shape[0] cols = pcd.shape[1] stride = 1 pcd_ = pcd.reshape((rows * cols, 3)) pcd_mat = MatrixDouble(pcd_) tri_mesh = extract_tri_mesh_from_organized_point_cloud( pcd_mat, rows, cols, stride) return tri_mesh
def test_100k_array_3d_lmax(benchmark, np_100K_array_3d, params_lmax): if Polylidar3D is not None: points_mat = MatrixDouble(np_100K_array_3d) pl = Polylidar3D(**params_lmax) _, _ , _ = benchmark(pl.extract_planes_and_polygons, points_mat) else: benchmark(extractPolygons, np_100K_array_3d, **params_lmax)
def extract_planes_and_polygons_from_classified_mesh( tri_mesh, avg_peaks, polylidar_kwargs=dict(alpha=0.0, lmax=0.1, min_triangles=2000, z_thresh=0.1, norm_thresh=0.95, norm_thresh_min=0.95, min_hole_vertices=50, task_threads=4), filter_polygons=True, pl_=None, optimized=True, postprocess=dict(filter=dict(hole_area=dict(min=0.025, max=100.0), hole_vertices=dict(min=6), plane_area=dict(min=0.05)), positive_buffer=0.00, negative_buffer=0.00, simplify=0.0)): if pl_ is not None: pl = pl_ else: pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized_classified( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 all_planes_shapely = [] all_obstacles_shapely = [] time_filter = [] # all_poly_lines = [] if filter_polygons: vertices = np.asarray(tri_mesh.vertices) for i in range(avg_peaks.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: planes_shapely, obstacles_shapely, filter_time = filter_and_create_polygons( vertices, polygons_for_normal, rm=rm, postprocess=postprocess) all_planes_shapely.extend(planes_shapely) all_obstacles_shapely.extend(obstacles_shapely) time_filter.append(filter_time) # all_poly_lines.extend(poly_lines) timings = dict(t_polylidar_planepoly=polylidar_time, t_polylidar_filter=np.array(time_filter).sum()) # all_planes_shapely, all_obstacles_shapely, all_poly_lines, timings return all_planes_shapely, all_obstacles_shapely, timings
def verify_all(points, polylidar_kwargs): pl = Polylidar3D(**polylidar_kwargs) points_mat = MatrixDouble(points) mesh, planes, polygons = pl.extract_planes_and_polygons(points_mat) # Basic test to ensure no obvious errors occurred basic_polylidar_verification(points, mesh, planes, polygons) # Ensure that the polygons returned are valid verify_all_polygons_are_valid(polygons, points)
def open_3d_mesh_to_trimesh(mesh: o3d.geometry.TriangleMesh): triangles = np.asarray(mesh.triangles) vertices = np.asarray(mesh.vertices) triangles = np.ascontiguousarray(triangles) vertices_mat = MatrixDouble(vertices) triangles_mat = MatrixInt(triangles) tri_mesh = create_tri_mesh_copy(vertices_mat, triangles_mat) return tri_mesh
def extract_all_dominant_planes(tri_mesh, vertices, polylidar_kwargs, config_pp, ds=50, min_samples=10000): ga = GaussianAccumulatorS2(level=4, max_phi=180) ico = IcoCharts(level=4) triangle_normals = np.asarray(tri_mesh.triangle_normals) num_normals = triangle_normals.shape[0] triangle_normals_ds = down_sample_normals(triangle_normals) # Get the data t0 = time.perf_counter() ga.integrate(MatX3d(triangle_normals_ds)) t1 = time.perf_counter() avg_peaks, _, _, timings = get_image_peaks(ico, ga, level=4, with_o3d=False) timings['t_fastga_integrate'] = (t1 - t0) * 1000 timings['t_fastga_total'] = timings['t_fastga_integrate'] + timings[ 't_fastga_peak'] logging.info("Processing mesh with %d triangles", num_normals) logging.info("Dominant Plane Normals") print(avg_peaks) avg_peaks_selected = np.copy(avg_peaks[[0, 1, 2, 3], :]) pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks_selected) # debugging purposes, ignore tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() timings['t_polylidar_planepoly'] = (t1 - t0) * 1000 all_poly_lines = [] for i in range(avg_peaks_selected.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons( vertices, polygons_for_normal, rm=rm, config_pp=config_pp) all_poly_lines.extend(poly_lines) return all_planes, tri_set, all_poly_lines, timings
def triangulate_and_combine(points, points_bad, triangles_bad): points_mat = MatrixDouble(points) mesh = Delaunator(points_mat) mesh.triangulate() triangles = np.asarray(mesh.triangles) points_new = np.concatenate([points, points_bad], axis=0) triangles_new = np.concatenate([triangles, triangles_bad], axis=0) all_planes = [np.arange(triangles_new.shape[0])] return mesh, points_new, triangles_new, all_planes
def make_uniform_grid_mesh(im, intrinsics, extrinsics, stride=2, **kwargs): """Create a Unifrom Grid Mesh from an RGBD Image Arguments: img {ndarray} -- MXN Float Depth Image intrinsics {ndarray} -- 3X3 intrinsics matrix extrinsics {ndarray} -- 4X4 matrix Keyword Arguments: stride {int} -- Stride for creating point cloud (default: {2}) Returns: tuple(dict, dict) - Mesh and timings """ t0 = time.perf_counter() tri_mesh = extract_tri_mesh_from_float_depth(MatrixFloat( im), MatrixDouble(intrinsics), MatrixDouble(extrinsics), stride=stride) t1 = time.perf_counter() return tri_mesh, (t1-t0) * 1000
def test_clusters(benchmark, cluster_groups): """ Will benchmark clusters AND check that python overhead is less than 3ms (ussually <1 ms) """ points = cluster_groups['points'] polylidar_kwargs = cluster_groups['polylidar_kwargs'] if Polylidar3D is not None: points_mat = MatrixDouble(points) pl = Polylidar3D(**polylidar_kwargs) _, _ , _ = benchmark(pl.extract_planes_and_polygons, points_mat) else: _, _ , _ = benchmark(extractPlanesAndPolygons, points, **polylidar_kwargs)
def open_3d_mesh_to_tri_mesh(mesh: o3d.geometry.TriangleMesh): triangles = np.asarray(mesh.triangles) vertices = np.asarray(mesh.vertices) vertices_mat = MatrixDouble(vertices) triangles_mat = MatrixInt(triangles) triangles_mat_np = np.asarray(triangles_mat) # print(triangles, triangles.dtype) # print(triangles_mat_np, triangles_mat_np.dtype) tri_mesh = create_tri_mesh_copy(vertices_mat, triangles_mat) return tri_mesh
def main(): a = np.arange(4, dtype=np.float64).reshape((2,2)) print("Numpy Array and Buffer Ptr") print(a) print(get_np_buffer_ptr(a)) b = MatrixDouble(a, False) c = np.asarray(b) print("") print("Matrix Array and Buffer Ptr") print(c) print(get_np_buffer_ptr(c)) try: d = a.reshape((4,)) print("") print("Reshape to 1 Dimension") print(d) print("Should throw runtime error because not 2 Dim") e = MatrixDouble(d, False) except Exception: print("Successfully threw an exception")
def main(): points = np.random.randn(1000, 2) # Use Delaunay Triangulation points_mat = MatrixDouble(points) delaunay = Delaunator(points_mat) delaunay.triangulate() triangles = np.copy(np.asarray(delaunay.triangles)) plt.triplot(points[:, 0], points[:, 1], triangles) plt.scatter(points[:, 0], points[:, 1]) plt.show()
def extract_all_dominant_planes(tri_mesh, vertices, polylidar_kwargs, ds=50, min_samples=10000): ga = GaussianAccumulatorS2(level=4, max_phi=180) triangle_normals = np.asarray(tri_mesh.triangle_normals) num_normals = triangle_normals.shape[0] # Downsample, TODO improve this ds_normals = int(num_normals / ds) to_sample = max(min([num_normals, min_samples]), ds_normals) ds_step = int(num_normals / to_sample) triangle_normals_ds = np.ascontiguousarray( triangle_normals[:num_normals:ds_step, :]) # A copy occurs here for triangle_normals......if done in c++ there would be no copy ga.integrate(MatX3d(triangle_normals_ds)) gaussian_normals = np.asarray(ga.get_bucket_normals()) accumulator_counts = np.asarray(ga.get_normalized_bucket_counts()) _, _, avg_peaks, _ = find_peaks_from_accumulator(gaussian_normals, accumulator_counts) logging.info("Processing mesh with %d triangles", num_normals) logging.info("Dominant Plane Normals") print(avg_peaks) avg_peaks_selected = np.copy(avg_peaks[[0, 1, 2, 3], :]) pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks_selected) tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 all_poly_lines = [] for i in range(avg_peaks_selected.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons( vertices, polygons_for_normal, rm=rm) all_poly_lines.extend(poly_lines) return all_planes, tri_set, all_poly_lines, polylidar_time
def get_polygon(points3D_cam, polylidar, postprocess_config): """Gets polygons from point cloud Arguments: points3D_cam {ndarray} -- Point cloud in Camera Frame Returns: (ndarray, ndarray, list[Polygons], list[Polygons], tuple(ints)) -- Rotated point cloud, rotation matrix from camera frame to rotated frame list of shapley polygons for planes and obstacles a tuple of execution times """ t0 = time.perf_counter() points3D_rot, rm = align_vector_to_axis( points3D_cam, np.array([0, 1, 0])) points3D_rot_ = np.ascontiguousarray(points3D_rot[:, :3]) logging.debug( "Extracting Polygons from point cloud of size: %d", points3D_rot.shape[0]) t1 = time.perf_counter() points_mat = MatrixDouble(points3D_rot_) # We need to perform these steps manually if we are going to pass a mesh instead of just the points # only necessary because I want the timings of just the frontend mesh = Delaunator(points_mat) mesh.triangulate() mesh.compute_triangle_normals() t1_2 = time.perf_counter() # bilateral_filter_normals(mesh, 5, 0.25, 0.25) t1_3 = time.perf_counter() planes, polygons = polylidar.extract_planes_and_polygons(mesh) t2 = time.perf_counter() planes, obstacles = filter_planes_and_holes2( polygons, points3D_rot_, postprocess_config) logging.debug("Number of Planes: %d; Number of obstacles: %d", len(planes), len(obstacles)) t3 = time.perf_counter() t_rotation = (t1 - t0) * 1000 t_polylidar = (t2 - t1) * 1000 t_polylidar_mesh = (t1_2 - t1) * 1000 t_polylidar_bilateral = (t1_3 - t1_2) * 1000 t_polylidar_planepoly = (t2 - t1_3) * 1000 t_polyfilter = (t3 - t2) * 1000 times = dict(t_rotation=t_rotation, t_polylidar_all=t_polylidar, t_polyfilter=t_polyfilter, t_polylidar_mesh=t_polylidar_mesh, t_polylidar_bilateral=t_polylidar_bilateral, t_polylidar_planepoly=t_polylidar_planepoly) return points3D_rot, rm, planes, obstacles, times
def extract_all_dominant_planes(tri_mesh, vertices, polylidar_kwargs, ds=50, min_samples=10000): ga = GaussianAccumulatorS2Beta(level=4) ico = IcoCharts(level=4) fast_ga_kwargs = dict(find_peaks_kwargs=dict(threshold_abs=15, min_distance=1, exclude_border=False, indices=False), cluster_kwargs=dict(t=0.28, criterion='distance'), average_filter=dict(min_total_weight=0.1)) avg_peaks, _, _, _, alg_timings = extract_all_dominant_plane_normals( tri_mesh, ga_=ga, ico_chart_=ico, **fast_ga_kwargs) logging.info("Dominant Plane Normals") print(avg_peaks) avg_peaks_selected = np.copy(avg_peaks[[0, 1, 2, 3, 4], :]) pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks_selected) tri_set = pl.extract_tri_set(tri_mesh, avg_peaks_mat) t0 = time.perf_counter() all_planes, all_polygons = pl.extract_planes_and_polygons_optimized( tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 all_poly_lines = [] for i in range(avg_peaks_selected.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons( vertices, polygons_for_normal, rm=rm) all_poly_lines.extend(poly_lines) return all_planes, tri_set, all_poly_lines, polylidar_time
def create_meshes_cuda(opc, **kwargs): """Creates a mesh from a noisy organized point cloud Arguments: opc {ndarray} -- Must be MXNX3 Keyword Arguments: loops {int} -- How many loop iterations (default: {5}) _lambda {float} -- weighted iteration movement (default: {0.5}) Returns: [tuple(mesh, timings)] -- polylidar mesh and timings """ smooth_opc, opc_normals = laplacian_then_bilateral_opc_cuda(opc, **kwargs) tri_mesh, tri_map = create_mesh_from_organized_point_cloud( smooth_opc, calc_normals=False) tri_norms = pick_valid_normals(opc_normals) opc_normals_cp = MatrixDouble(tri_norms, copy=True) # copy here!!!!! # plot_triangle_normals(np.asarray(tri_mesh.triangle_normals), opc_normals) tri_mesh.set_triangle_normals(opc_normals_cp) # copy again here....sad return tri_mesh
def extract_planes_and_polygons_from_mesh(tri_mesh, avg_peaks, polylidar_kwargs=dict(alpha=0.0, lmax=0.1, min_triangles=2000, z_thresh=0.1, norm_thresh=0.95, norm_thresh_min=0.95, min_hole_vertices=50, task_threads=4), filter_polygons=True, pl_=None, optimized=True, **kwargs): if pl_ is not None: pl = pl_ else: pl = Polylidar3D(**polylidar_kwargs) avg_peaks_mat = MatrixDouble(avg_peaks) t0 = time.perf_counter() if optimized: all_planes, all_polygons = pl.extract_planes_and_polygons_optimized(tri_mesh, avg_peaks_mat) else: all_planes, all_polygons = pl.extract_planes_and_polygons(tri_mesh, avg_peaks_mat) t1 = time.perf_counter() polylidar_time = (t1 - t0) * 1000 logger.info("Polygon Extraction - Took (ms): %.2f", polylidar_time) all_poly_lines = [] if filter_polygons: vertices = np.asarray(tri_mesh.vertices) for i in range(avg_peaks.shape[0]): avg_peak = avg_peaks[i, :] rm, _ = R.align_vectors([[0, 0, 1]], [avg_peak]) polygons_for_normal = all_polygons[i] # print(polygons_for_normal) if len(polygons_for_normal) > 0: poly_lines, _ = filter_and_create_open3d_polygons(vertices, polygons_for_normal, rm=rm, **kwargs) all_poly_lines.extend(poly_lines) timings = dict(polylidar=polylidar_time) return all_planes, all_polygons, all_poly_lines, timings
def main(): kwargs = dict(num_groups=2, group_size=1000, dist=100.0, seed=1) # generate random normally distributed clusters of points, 200 X 2 numpy array. points = generate_test_points(**kwargs) lmax = get_estimated_lmax(**kwargs) polylidar_kwargs = dict(alpha=0.0, lmax=lmax, min_triangles=5) # print(polylidar_kwargs) # Convert Points and make Polylidar points_mat = MatrixDouble(points) polylidar = Polylidar3D(**polylidar_kwargs) # Extracts planes and polygons, time t1 = time.perf_counter() mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.perf_counter() triangles = np.asarray(mesh.triangles) # print(triangles, triangles.shape) triangles = triangles.flatten() # print(triangles, triangles.shape) planes_np = np.asarray(planes) # print(planes_np) print("Took {:.2f} milliseconds".format((t2 - t1) * 1000)) # Plot Data if points.shape[0] < 100000: fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1) # plot points print("Plot Points") ax.scatter(points[:, 0], points[:, 1], c='k', s=20.0) fig.savefig("assets/scratch/Basic2DAlgorithm_pointcloud.png", bbox_inches='tight', pad_inches=-0.6) plt.show() print("Plot Mesh") fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1) # plot points ax.scatter(points[:, 0], points[:, 1], c='k', s=0.1) # plot all triangles plot_triangles(get_triangles_from_list(triangles, points), ax) fig.savefig("assets/scratch/Basic2DAlgorithm_mesh.png", bbox_inches='tight', pad_inches=-0.6) plt.show() print("Planes and Polygons") fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1) # plot points ax.scatter(points[:, 0], points[:, 1], c='k', s=0.1) # plot all triangles plot_triangles(get_triangles_from_list(triangles, points), ax) # plot mesh triangles triangle_meshes = get_colored_planar_segments(planes_np, triangles, points) plot_triangle_meshes(triangle_meshes, ax) # plot polygons plot_polygons(polygons, points, ax) plt.axis('equal') plt.show()
from surfacedetector.utility.helper_mesh import create_meshes_cuda, create_meshes_cuda_with_o3d, create_meshes from surfacedetector.utility.helper_polylidar import extract_all_dominant_plane_normals, extract_planes_and_polygons_from_mesh from surfacedetector.utility.helper_tracking import get_pose_matrix, cycle_pose_frames, callback_pose from surfacedetector.utility.helper_wheelchair import analyze_planes logging.basicConfig(level=logging.INFO) THIS_DIR = path.dirname(__file__) CONFIG_DIR = path.join(THIS_DIR, "config") ASSETS_DIR = path.join(THIS_DIR, '..', 'assets') VID_DIR = path.join(ASSETS_DIR, 'videos') PICS_DIR = path.join(ASSETS_DIR, 'pics') DEFAULT_CONFIG_FILE = path.join(CONFIG_DIR, "default.yaml") IDENTITY = np.identity(3) IDENTITY_MAT = MatrixDouble(IDENTITY) axis = o3d.geometry.TriangleMesh.create_coordinate_frame() ser = serial.Serial('/dev/ttyACM0', 9600, timeout=1) def create_pipeline(config: dict): """Sets up the pipeline to extract depth and rgb frames Arguments: config {dict} -- A dictionary mapping for configuration. see default.yaml Returns: tuple -- pipeline, process modules, filters, t265 device (optional) """ # Create pipeline and config for D4XX,L5XX
def main(): points = generate_point_cloud() points_mat = MatrixDouble(points) polylidar_kwargs = dict(alpha=0.0, lmax=1.0, min_triangles=20, z_thresh=0.2, norm_thresh=0.98) polylidar = Polylidar3D(**polylidar_kwargs) t1 = time.perf_counter() # Create Pseudo 3D Surface Mesh using Delaunay Triangulation and Polylidar mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.perf_counter() print("Point Size: {}".format(points.shape[0])) print( "2.5D Delaunay Triangulation and Plane Extraction; Mesh Creation {:.2f} milliseconds" .format((t2 - t1) * 1000)) # Visualize # Create Open3D Mesh mesh_planes = extract_mesh_planes(points, mesh, planes, COLOR_PALETTE[0]) # Create Open 3D Point Cloud pcd = create_open3d_pc(points) o3d.visualization.draw_geometries([pcd, *mesh_planes]) # Estimate Point Cloud Normals t0 = time.perf_counter() pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( radius=1.0, max_nn=8)) t1 = time.perf_counter() # Create True 3D Surface Mesh using Ball Pivot Algorithm radii = o3d.utility.DoubleVector([0.4, 0.5]) mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting( pcd, radii) mesh.paint_uniform_color(COLOR_PALETTE[0]) t2 = time.perf_counter() print( "3D Triangulation using Ball Pivot: Normal Estimation took: {:.2f}, Mesh Creation: {:.2f}" .format((t1 - t0) * 1000, (t2 - t1) * 1000)) # Visualize o3d.visualization.draw_geometries([pcd, mesh]) # Create True 3D Surface Mesh using Poisson Reconstruction Algorithm t3 = time.perf_counter() mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson( pcd, depth=6) t4 = time.perf_counter() print( "3D Triangulation using Poisson Reconstruction: Mesh Creation: {:.2f}". format((t4 - t3) * 1000)) # Visualize # o3d.visualization.draw_geometries([mesh]) # print(mesh) # print('visualize densities') densities = np.asarray(densities) density_colors = plt.get_cmap('plasma')( (densities - densities.min()) / (densities.max() - densities.min())) density_colors = density_colors[:, :3] density_mesh = o3d.geometry.TriangleMesh() density_mesh.vertices = mesh.vertices density_mesh.triangles = mesh.triangles density_mesh.triangle_normals = mesh.triangle_normals density_mesh.vertex_colors = o3d.utility.Vector3dVector(density_colors) # mesh.vertex_colors = o3d.utility.Vector3dVector(density_colors) # o3d.visualization.draw_geometries([density_mesh]) # print('remove low density vertices') vertices_to_remove = densities < np.quantile(densities, 0.1) mesh.remove_vertices_by_mask(vertices_to_remove) mesh.compute_triangle_normals() mesh.paint_uniform_color(COLOR_PALETTE[0]) # print(mesh) o3d.visualization.draw_geometries([pcd, mesh])
def main(): np.random.seed(1) # generate random plane with hole plane = generate_3d_plane(bounds_x=[0, 10, 0.5], bounds_y=[0, 10, 0.5], holes=[ [[3, 5], [3, 5]]], height_noise=0.02, planar_noise=0.02) # Generate top of box (causing the hole that we see) box_top = generate_3d_plane(bounds_x=[3, 5, 0.2], bounds_y=[3, 5, 0.2], holes=[ ], height_noise=0.02, height=2, planar_noise=0.02) # Generate side of box (causing the hole that we see) box_side = generate_3d_plane(bounds_x=[0, 2, 0.2], bounds_y=[ 0, 2, 0.2], holes=[], height_noise=0.02, planar_noise=0.02) rm = rotation_matrix([0, 1, 0], -math.pi / 2.0) box_side = apply_rotation(rm, box_side) + [5, 3, 0] # All points joined together points = np.concatenate((plane, box_side, box_top)) points_mat = MatrixDouble(points) polylidar_kwargs = dict(alpha=0.0, lmax=1.0, min_triangles=20, z_thresh=0.1, norm_thresh_min=0.94) polylidar = Polylidar3D(**polylidar_kwargs) elev = 15.0 azim = -35 # Show Point Cloud print("Should see point raw point cloud") fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1, subplot_kw=dict(projection='3d')) # plot points ax.scatter(*scale_points(points), s=20.0, c=points[:, 2], cmap=plt.cm.plasma) set_axes_equal(ax) ax.view_init(elev=elev, azim=azim) fig.savefig("assets/scratch/Basic25DAlgorithm_pointcloud.pdf", bbox_inches='tight') fig.savefig("assets/scratch/Basic25DAlgorithm_pointcloud.png", bbox_inches='tight', pad_inches=-0.8) plt.show() # Extracts planes and polygons, time t1 = time.time() mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.time() print("Polylidar Took {:.2f} milliseconds".format((t2 - t1) * 1000)) triangles = np.asarray(mesh.triangles) all_planes = [np.arange(triangles.shape[0])] # Show Triangulation fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1, subplot_kw=dict(projection='3d')) plot_planes_3d(points, triangles, all_planes, ax, alpha=0.0, z_value=-4.0) plot_planes_3d(points, triangles, all_planes, ax, alpha=0.5) # plot points ax.scatter(*scale_points(points), s=0.1, c='k') set_axes_equal(ax, ignore_z=True) ax.set_zlim3d([-4, 6]) ax.view_init(elev=elev, azim=azim) print("Should see triangulation point cloud") fig.savefig("assets/scratch/Basic25DAlgorithm_mesh.pdf", bbox_inches='tight') fig.savefig("assets/scratch/Basic25DAlgorithm_mesh.png", bbox_inches='tight', pad_inches=-0.8) plt.show() print("") print("Should see two planes extracted, please rotate.") fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1, subplot_kw=dict(projection='3d')) # plot all triangles plot_polygons_3d(points, polygons, ax) plot_planes_3d(points, triangles, planes, ax) # plot points ax.scatter(*scale_points(points), c='k', s=0.1) set_axes_equal(ax) ax.view_init(elev=elev, azim=azim) fig.savefig("assets/scratch/Basic25DAlgorithm_polygons.pdf", bbox_inches='tight') fig.savefig("assets/scratch/Basic25DAlgorithm_polygons.png", bbox_inches='tight', pad_inches=-0.8) plt.show() print("")
def main(): np.random.seed(1) # generate random plane with hole plane = generate_3d_plane(bounds_x=[0, 10, 0.5], bounds_y=[0, 10, 0.5], holes=[[[3, 5], [3, 5]]], height_noise=0.02, planar_noise=0.02) # Generate top of box (causing the hole that we see) box_top = generate_3d_plane(bounds_x=[3, 5, 0.2], bounds_y=[3, 5, 0.2], holes=[], height_noise=0.02, height=2, planar_noise=0.02) # Generate side of box (causing the hole that we see) box_side = generate_3d_plane(bounds_x=[0, 2, 0.2], bounds_y=[0, 2, 0.2], holes=[], height_noise=0.02, planar_noise=0.02) rm = rotation_matrix([0, 1, 0], -math.pi / 2.0) box_side = apply_rotation(rm, box_side) + [5, 3, 0] # All points joined together points = np.concatenate((plane, box_side, box_top)) points_mat = MatrixDouble(points) polylidar_kwargs = dict(alpha=0.0, lmax=1.0, min_triangles=20, z_thresh=0.1, norm_thresh_min=0.94) polylidar = Polylidar3D(**polylidar_kwargs) fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1, subplot_kw=dict(projection='3d')) # plot points ax.scatter(*scale_points(points), s=2.5, c=points[:, 2], cmap=plt.cm.plasma) set_axes_equal(ax) ax.view_init(elev=15., azim=-35) plt.show() # Extracts planes and polygons, time t1 = time.time() mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.time() print("Took {:.2f} milliseconds".format((t2 - t1) * 1000)) print("Should see two planes extracted, please rotate.") triangles = np.asarray(mesh.triangles) fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1, subplot_kw=dict(projection='3d')) # plot all triangles plot_planes_3d(points, triangles, planes, ax) plot_polygons_3d(points, polygons, ax) # plot points ax.scatter(*scale_points(points), c='k', s=0.1) set_axes_equal(ax) ax.view_init(elev=15., azim=-35) plt.show() print("")
def run_test(pcd, rgbd, intrinsics, extrinsics, bp_alg=dict(radii=[0.02, 0.02]), poisson=dict(depth=8), callback=None, stride=2): """Demonstrate Polygon Extraction on both unorganized and organized point cloud Args: pcd (o3d.geometry.PointCloud): Open3D point cloud rgbd (np.ndarray): MXN Numpy array intrinsics (np.ndarray): 3X3 numpy array of camera intrinsics (assume pin hole model) extrinsics (np.ndarray): 4X4 numpy array of extrinsics of camera bp_alg (dict, optional): Arguments to Open3D ball pivot alg. Defaults to dict(radii=[0.02, 0.02]). poisson (dict, optional): Arguments to Open3D Poisson surface reconstruction. Defaults to dict(depth=8). callback (function, optional): Callback function for visualization. Defaults to None. stride (int, optional): Skip rows/columns in rgbd. Defaults to 2. """ points = np.asarray(pcd.points) polylidar_kwargs = dict(alpha=0.0, lmax=0.10, min_triangles=100, z_thresh=0.04, norm_thresh=0.90, norm_thresh_min=0.90, min_hole_vertices=6) pl = Polylidar3D(**polylidar_kwargs) ################################################################################ ##### Treat data as an unorganized point clouds ##### Create Surface Mesh using 2.5 Delaunay Triangulation and extract Polygons ################################################################################ points_mat = MatrixDouble(points) t1 = time.perf_counter() mesh, planes, polygons = pl.extract_planes_and_polygons(points_mat) t2 = time.perf_counter() # Visualization of mesh and polygons all_poly_lines = filter_and_create_open3d_polygons(points, polygons) triangles = np.asarray(mesh.triangles) mesh_2d_polylidar = extract_mesh_planes(points, triangles, planes, mesh.counter_clock_wise, COLOR_PALETTE[0]) mesh_2d_polylidar.extend(flatten([line_mesh.cylinder_segments for line_mesh in all_poly_lines])) time_mesh_2d_polylidar = (t2 - t1) * 1000 polylidar_alg_name = 'Treated as **Unorganized** Point Cloud - 2.5D Delaunay Triangulation with Polygon Extraction' callback(polylidar_alg_name, time_mesh_2d_polylidar, pcd, mesh_2d_polylidar) ################################################################################ ###### Treat data as an **Organized** 3D Point Cloud ######### ###### Creates a true 3D mesh and is much master using organized structure of point cloud ################################################################################ tri_mesh, t_mesh_creation = make_uniform_grid_mesh(np.asarray( rgbd.depth), np.ascontiguousarray(intrinsics.intrinsic_matrix), extrinsics, stride=stride) # Visualization of only the mesh tri_mesh_o3d = create_open_3d_mesh_from_tri_mesh(tri_mesh) uniform_alg_name = 'Treated as **Organized** Point Cloud - Right-Cut Triangulation/Uniform Mesh (Mesh only)' callback(uniform_alg_name, t_mesh_creation, pcd, tri_mesh_o3d) # Exctact Polygons with Polylidar3D using the Uniform Mesh. Dominant Plane normal is 0,0,1. t1 = time.perf_counter() planes, polygons = pl.extract_planes_and_polygons(tri_mesh) t2 = time.perf_counter() # Visualization of mesh and polygons vertices_np = np.asarray(tri_mesh.vertices) triangles_np = np.asarray(tri_mesh.triangles) all_poly_lines = filter_and_create_open3d_polygons(vertices_np, polygons) mesh_3d_polylidar = extract_mesh_planes(vertices_np, triangles_np, planes, tri_mesh.counter_clock_wise) mesh_3d_polylidar.extend(flatten([line_mesh.cylinder_segments for line_mesh in all_poly_lines])) time_polylidar3D = (t2 - t1) * 1000 polylidar_3d_alg_name = 'Polygon Extraction on Uniform Mesh (only one dominant plane normal)' callback(polylidar_3d_alg_name, time_polylidar3D, create_open3d_pc(vertices_np), mesh_3d_polylidar)
def main(): # np.random.seed(5) np.random.seed(9) # generate random plane with hole plane = generate_3d_plane(bounds_x=[0, 10, 0.5], bounds_y=[0, 10, 0.5], holes=[[[3, 5], [3, 5]]], height_noise=0.05, planar_noise=0.10) # Generate top of box (causing the hole that we see) box_top = generate_3d_plane(bounds_x=[3, 5, 0.2], bounds_y=[3, 5, 0.2], holes=[], height_noise=0.02, height=2, planar_noise=0.02) # Generate side of box (causing the hole that we see) box_side = generate_3d_plane(bounds_x=[0, 2, 0.2], bounds_y=[0, 2, 0.2], holes=[], height_noise=0.02, planar_noise=0.02) rm = rotation_matrix([0, 1, 0], -math.pi / 2.0) box_side = apply_rotation(rm, box_side) + [5, 3, 0] # box_side = r.apply(box_side) + [5, 3, 0] # All points joined together points = np.concatenate((plane, box_side, box_top)) points_mat = MatrixDouble(points) polylidar_kwargs = dict(alpha=0.0, lmax=1.0, min_triangles=20, z_thresh=0.1, norm_thresh_min=0.94) polylidar = Polylidar3D(**polylidar_kwargs) elev = 15.0 azim = -125 # Extracts planes and polygons, time t1 = time.time() mesh, planes, polygons = polylidar.extract_planes_and_polygons(points_mat) t2 = time.time() print("Polylidar Took {:.2f} milliseconds".format((t2 - t1) * 1000)) triangles = np.asarray(mesh.triangles) all_planes = [np.arange(triangles.shape[0])] print("") print("Should see a mesh segments") fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1, subplot_kw=dict(projection='3d')) # plot all triangles # plot_polygons_3d(points, polygons, ax) rm_45 = rotation_matrix([0, 1, 0], -math.pi / 4.0) points_rot = apply_rotation(rm_45, points) plot_planes_3d(points_rot, triangles, [planes[1]], ax, alpha=[0.85]) # plot points # ax.scatter(*scale_points(points), c='k', s=0.1) set_axes_equal(ax) ax.view_init(elev=elev, azim=azim) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') fig.savefig("assets/scratch/PolygonExtraction_a.pdf", bbox_inches='tight') # fig.savefig("assets/scratch/Basic25DAlgorithm_polygons.png", bbox_inches='tight', pad_inches=-0.8) plt.show() # Show 2D Projection and Polygon Extraction print("") print("Should see projected vertices to geometric plane") fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(5, 5)) simplices_plane = triangles[planes[1], :] plot_polygons([polygons[1]], points[:, :2], ax, shell_color=COLOR_PALETTE[4], hole_color=COLOR_PALETTE[4]) ax.triplot(points[:, 0], points[:, 1], simplices_plane, c='k', alpha=0.75) xlim = ax.get_xlim() ylim = ax.get_ylim() ax.set_xlabel('X') ax.set_ylabel('Y') fig.savefig("assets/scratch/PolygonExtraction_b1.pdf", bbox_inches='tight') plt.show() # Show 2D Projection and Polygon Extraction print("") print("Should see projected vertices to geometric plane") fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(5, 5)) simplices_plane = triangles[planes[1], :] plot_polygons([polygons[1]], points[:, :2], ax) ax.triplot(points[:, 0], points[:, 1], simplices_plane, c='k', alpha=0.75) ax.set_xlabel('X') ax.set_ylabel('Y') fig.savefig("assets/scratch/PolygonExtraction_b2.pdf", bbox_inches='tight') plt.show() poly = polygons[1] shell_coords = get_points(poly.shell, points) hole_coords = [get_points(hole, points) for hole in poly.holes] poly_shape = Polygon(shell=shell_coords, holes=hole_coords) # Show Simplification print("") print("Should see simplified Polygon") fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(5, 5)) patch = PolygonPatch(poly_shape, fill=True, alpha=0.5) ax.add_patch(patch) # plot_polygons([polygons[1]], points[:, :2], ax) poly_simplify = poly_shape.simplify(.2, preserve_topology=True) patch = PolygonPatch(poly_simplify, color='k', fill=False, linewidth=1.5, linestyle='--', zorder=1) ax.add_patch(patch) ax.set_xlim(xlim) ax.set_ylim(ylim) print(f"Before Vertices: {len(poly_shape.exterior.coords)}") print(f"After Vertices: {len(poly_simplify.exterior.coords)}") ax.set_xlabel('X') ax.set_ylabel('Y') fig.savefig("assets/scratch/PolygonExtraction_c.pdf", bbox_inches='tight') plt.show() # Show Positive Buffer print("") print("Should see positive buffer Polygon") fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(5, 5)) patch = PolygonPatch(poly_shape, fill=True, alpha=0.5) ax.add_patch(patch) # plot_polygons([polygons[1]], points[:, :2], ax) poly_buffer = poly_simplify.buffer(.2) patch = PolygonPatch(poly_buffer, color='k', fill=False, linewidth=1.5, linestyle='--', zorder=1) ax.add_patch(patch) ax.set_xlim(xlim) ax.set_ylim(ylim) print(f"After Vertices: {len(poly_buffer.exterior.coords)}") ax.set_xlabel('X') ax.set_ylabel('Y') fig.savefig("assets/scratch/PolygonExtraction_d.pdf", bbox_inches='tight') plt.show() # Show Negative Buffer print("") print("Should see negative buffer Polygon") fig, ax = plt.subplots(nrows=1, ncols=1, figsize=(5, 5)) patch = PolygonPatch(poly_shape, fill=True, alpha=0.5) ax.add_patch(patch) # plot_polygons([polygons[1]], points[:, :2], ax) poly_buffer = poly_buffer.buffer(-.2) patch = PolygonPatch(poly_buffer, color='k', fill=False, linewidth=1.5, linestyle='--', zorder=1) ax.add_patch(patch) ax.set_xlim(xlim) ax.set_ylim(ylim) print(f"After Vertices: {len(poly_buffer.exterior.coords)}") ax.set_xlabel('X') ax.set_ylabel('Y') fig.savefig("assets/scratch/PolygonExtraction_e.pdf", bbox_inches='tight') plt.show()