def main():
normals = np.array([[-1, 0, 0], [0, 0, 1], [1, 0, 0], [0, 1, 0],
[0, -1, 0]])
ga = GaussianAccumulatorOpt(level=4)
proj, hilbert_values = convert_normals_to_hilbert(MatX3d(normals),
ga.projected_bbox)
s2_values = convert_normals_to_s2id(MatX3d(normals))
print("Normals: ")
print(normals)
print(
"Transformed to uint32 hilbert values (azimuth equal area projection at northpole):"
)
print(hilbert_values)
print(
"S2ID. Cubic Projection on sphere. Each face has its own hilbert value computed:"
)
print(np.asarray(s2_values))
def extract_all_dominant_plane_normals(tri_mesh,
level=5,
with_o3d=False,
ga_=None,
ico_chart_=None,
**kwargs):
# Reuse objects if provided
if ga_ is not None:
ga = ga_
else:
ga = GaussianAccumulatorS2(level=level)
if ico_chart_ is not None:
ico_chart = ico_chart_
else:
ico_chart = IcoCharts(level=level)
triangle_normals = np.asarray(tri_mesh.triangle_normals)
triangle_normals_ds = down_sample_normals(triangle_normals, **kwargs)
# np.savetxt('bad_normals.txt', triangle_normals_ds)
triangle_normals_ds_mat = MatX3d(triangle_normals_ds)
t1 = time.perf_counter()
ga.integrate(triangle_normals_ds_mat)
t2 = time.perf_counter()
logging.debug(
"Gaussian Accumulator - Normals Sampled: %d; Took (ms): %.2f",
triangle_normals_ds.shape[0], (t2 - t1) * 1000)
avg_peaks, pcd_all_peaks, arrow_avg_peaks, timings_dict = get_image_peaks(
ico_chart, ga, level=level, with_o3d=with_o3d, **kwargs)
# Create Open3D structures for visualization
if with_o3d:
# Visualize the Sphere
accumulator_counts = np.asarray(ga.get_normalized_bucket_counts())
refined_icosahedron_mesh = create_open_3d_mesh(
np.asarray(ga.mesh.triangles), np.asarray(ga.mesh.vertices))
color_counts = get_colors(accumulator_counts)[:, :3]
colored_icosahedron = assign_vertex_colors(refined_icosahedron_mesh,
color_counts)
else:
colored_icosahedron = None
elapsed_time_fastga = (t2 - t1) * 1000
elapsed_time_peak = timings_dict['t_fastga_peak']
elapsed_time_total = elapsed_time_fastga + elapsed_time_peak
timings = dict(t_fastga_total=elapsed_time_total,
t_fastga_integrate=elapsed_time_fastga,
t_fastga_peak=elapsed_time_peak)
ga.clear_count()
return avg_peaks, pcd_all_peaks, arrow_avg_peaks, colored_icosahedron, timings
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 visualize_gaussian_integration(ga: GaussianAccumulatorKDPy,
mesh: o3d.geometry.TriangleMesh,
ds=50,
min_samples=10000,
max_phi=100,
integrate_kwargs=dict()):
num_buckets = ga.num_buckets
to_integrate_normals = np.asarray(mesh.triangle_normals)
# remove normals on bottom half of sphere
to_integrate_normals, _ = filter_normals_by_phi(to_integrate_normals,
max_phi=180)
# determine optimal sampling
num_normals = to_integrate_normals.shape[0]
ds_normals = int(num_normals / ds)
to_sample = max(min([num_normals, min_samples]), ds_normals)
ds_step = int(num_normals / to_sample)
# perform sampling of normals
to_integrate_normals = to_integrate_normals[0:num_normals:ds_step, :]
mask = np.asarray(ga.mask)
query_size = to_integrate_normals.shape[0]
mask = np.ones((np.asarray(ga.mesh.triangles).shape[0], ), dtype=bool)
mask[num_buckets:] = False
class_name_str = type(ga).__name__
# integrate normals
if class_name_str in [
'GaussianAccumulatorKD', 'GaussianAccumulatorOpt',
'GaussianAccumulatorS2'
]:
to_integrate_normals = MatX3d(to_integrate_normals)
# mask = np.ma.make_mask(mask)
# triangles = np.asarray(ga.mesh.triangles)
t0 = time.perf_counter()
neighbors_idx = np.asarray(
ga.integrate(to_integrate_normals, **integrate_kwargs))
t1 = time.perf_counter()
elapsed_time = (t1 - t0) * 1000
print(
"{}; KD tree size: {}; Query Size (K): {}; Execution Time(ms): {:.1f}".
format(class_name_str, num_buckets, query_size, elapsed_time))
normalized_counts = np.asarray(ga.get_normalized_bucket_counts())
color_counts = get_colors(normalized_counts)[:, :3]
# print(normalized_counts)
refined_icosahedron_mesh = create_open_3d_mesh(
np.asarray(ga.mesh.triangles), np.asarray(ga.mesh.vertices))
# Colorize normal buckets
colored_icosahedron = assign_vertex_colors(refined_icosahedron_mesh,
color_counts, mask)
return colored_icosahedron, np.asarray(to_integrate_normals), neighbors_idx
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 integrate_normals_and_visualize(to_integrate_normals, ga):
to_integrate_normals_mat = MatX3d(to_integrate_normals)
t0 = time.perf_counter()
neighbors_idx = np.asarray(ga.integrate(to_integrate_normals_mat))
t1 = time.perf_counter()
elapsed_time = (t1 - t0) * 1000
normalized_counts = np.asarray(ga.get_normalized_bucket_counts())
color_counts = get_colors(normalized_counts)[:, :3]
refined_icosahedron_mesh = create_open_3d_mesh(
np.asarray(ga.mesh.triangles), np.asarray(ga.mesh.vertices))
# Colorize normal buckets
colored_icosahedron = assign_vertex_colors(refined_icosahedron_mesh,
color_counts, None)
return colored_icosahedron
def main():
normals = np.array([
[-1, 0, 0],
[0, 0, 1],
[1, 0, 0],
[0, 1, 0],
[0, -1, 0]
])
t0 = time.perf_counter()
ga = GaussianAccumulatorKD(level=0, max_phi=180)
t1 = time.perf_counter()
print("Number of Buckets: {}\n".format(len(ga.buckets)))
print("Bucket representations:\n {}\n".format(ga.buckets))
print("Bucket Cell Surface Normals: \n {}\n".format(np.asarray(ga.get_bucket_normals())))
normals_mat = MatX3d(normals) # need to convert a format we understand
t2 = time.perf_counter()
bucket_indexes = ga.integrate(normals_mat)
t3 = time.perf_counter()
print("These normals: \n {} \n are most similar to these cell normlas: \n {} \n".format(normals, np.asarray(ga.get_bucket_normals())[bucket_indexes,:]))
print(np.asarray(bucket_indexes))
print("Building Index Took (ms): {}; Query Took (ms): {}".format((t1-t0) * 1000, (t3 - t2)* 1000))
print("Change the level see a better approximation")
def plot_issues_2(idx, normals, chosen_buckets, ga, mesh):
normal_idx = idx[0]
bad_normals = normals[idx, :]
bad_chosen_triangles = chosen_buckets[idx]
chosen_triangle = bad_chosen_triangles[0]
bad_normal = np.expand_dims(bad_normals[0, :], axis=0)
print(bad_normal)
# chosen_normal =
# Get all bucket normals
bucket_normals_sorted = np.asarray(ga.get_bucket_normals())
# Get 2D projection of bad normal
projected_normals, hv = convert_normals_to_hilbert(MatX3d(normals),
ga.projected_bbox)
projected_normals = np.asarray(projected_normals)
bad_projected_normal = projected_normals[normal_idx, :]
# Get Projected coordinates of buckets
proj = np.asarray(ga.get_bucket_projection())
normalized_counts = np.asarray(ga.get_normalized_bucket_counts())
colors = get_colors(normalized_counts)[:, :3]
# Get Projected Coordinates of Buckets and normals!
all_normals = np.vstack((bucket_normals_sorted, normals))
all_projected_normals, all_hv = convert_normals_to_hilbert(
MatX3d(all_normals), ga.projected_bbox)
all_projected_normals = np.asarray(all_projected_normals)
all_hv = np.asarray(all_hv)
idx_sort = np.argsort(all_hv)
all_projected_normals = all_projected_normals[idx_sort]
all_normals_sorted = np.ascontiguousarray(all_normals[idx_sort])
fig, axs = plt.subplots(1, 1, figsize=(5, 5))
ax = axs
# plot buckets
scatter1 = ax.scatter(proj[:, 0],
proj[:, 1],
c=colors,
label='Projected Buckets')
scatter2 = ax.scatter(proj[chosen_triangle, 0],
proj[chosen_triangle, 1],
c='r',
label='Chosen Triangle')
# scatter3 = ax.scatter(proj[133, 0], proj[133, 1], c='g', label='Hilbert Mapped Triangle')
scatter4 = ax.scatter(bad_projected_normal[0],
bad_projected_normal[1],
c=[[0.5, 0.5, 0.5]],
label='Projected Normal')
line1 = ax.plot(all_projected_normals[:, 0],
all_projected_normals[:, 1],
c='k',
label='Hilbert Curve Connections')[0]
leg = ax.legend(loc='upper left', fancybox=True, shadow=True)
# scatter2 = ax.scatter(proj[133, 0], proj[133, 1], c='k')
plt.show()
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(bad_normal)
pcd.colors = o3d.utility.Vector3dVector([[0.5, 0.5, 0.5]])
pcd2 = o3d.geometry.PointCloud()
pcd2.points = o3d.utility.Vector3dVector(bucket_normals_sorted)
pcd2.colors = o3d.utility.Vector3dVector([[0.0, 1.0, 0]])
vertex_colors = np.asarray(mesh.vertex_colors)
triangles = np.asarray(mesh.triangles)
p_idx = triangles[chosen_triangle, :]
vertex_colors[p_idx] = [1, 0, 0]
# p_idx = triangles[133,:]
# vertex_colors[p_idx] = [0, 1, 0]
ls = create_line_set(all_normals_sorted * 1.002)
o3d.visualization.draw_geometries([mesh, pcd, pcd2, ls])