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()
