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 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 __init__(self, config):
super().__init__()
self.data_folder: str = config['data_folder']
self.date: str = config['date']
self.drive: str = config['drive']
self.frames = config['frames']
self.view_image = config['view_image']
self.cam = config['cam']
self.pointcloud = config['pointcloud']
self.view_3D = config['view_3D']
self.polylidar_kwargs = config['polygon']['polylidar']
self.postprocess = config['polygon']['postprocess']
self.record = config['record']
stacked_str = "_stacked" if self.record['stacked'] and self.view_3D[
'active'] else ""
self.record['fpath'] = str(
Path(self.record['directory']) /
"{}_{}{}.avi".format(self.date, self.drive, stacked_str))
self.interactive = config['interactive']
self.load_kitti(self.data_folder,
self.date,
self.drive,
frames=self.frames)
self.cm = plt.get_cmap(self.pointcloud['color_map'])
self.frame_iter = range(len(self.data_kitti.velo_files))
self.polylidar = Polylidar3D(**self.polylidar_kwargs)
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 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_polygons(opc, classes, config):
pl = Polylidar3D(**config['polylidar'])
ga = GaussianAccumulatorS2Beta(level=config['fastga']['level'])
ico = IcoCharts(level=config['fastga']['level'])
# 1. Create mesh
tri_mesh = create_meshes_cuda(opc, **config['mesh']['filter'])
mesh_o3d = create_open_3d_mesh_from_tri_mesh(tri_mesh)
classes = classes.reshape((np.asarray(mesh_o3d.vertices).shape[0], 1))
classes_mat = MatrixUInt8(classes)
tri_mesh.set_vertex_classes(classes_mat, True)
# alg_timings.update(timings)
# 2. Get dominant plane normals
avg_peaks, _, _, _, alg_timings = extract_all_dominant_plane_normals(
tri_mesh, ga_=ga, ico_chart_=ico, **config['fastga'])
# alg_timings.update(timings)
# 3. Extract Planes and Polygons
planes, obstacles, timings = extract_planes_and_polygons_from_classified_mesh(
tri_mesh,
avg_peaks,
pl_=pl,
filter_polygons=True,
optimized=True,
postprocess=config['polygon']['postprocess'])
return mesh_o3d, planes, obstacles, timings
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 get_planes(pl: polylidar.Polylidar3D, lidar_building, class_id, class_label, config_pp, **kwargs):
np_mat = polylidar.MatrixDouble(lidar_building)
mesh, planes, polygons = pl.extract_planes_and_polygons(np_mat)
planes = filter_planes(polygons, lidar_building, config_pp)
features = []
for plane in planes:
features.append(Feature(plane[0], {'class_id': class_id, 'class_label': class_label, 'height': plane[1]}))
return features
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 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 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 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 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 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()
def capture(config, video=None):
# Configure streams
pipeline, process_modules, filters, proj_mat, t265_device = create_pipeline(
config)
t265_pipeline = t265_device['pipeline']
logging.info("Pipeline Created")
# Long lived objects. These are the object that hold all the algorithms for surface exraction.
# They need to be long lived (objects) because they hold state (thread scheduler, image datastructures, etc.)
ll_objects = dict()
ll_objects['pl'] = Polylidar3D(**config['polylidar'])
ll_objects['ga'] = GaussianAccumulatorS2(level=config['fastga']['level'])
ll_objects['ico'] = IcoCharts(level=config['fastga']['level'])
if video:
frame_width = config['color']['width'] * 2
frame_height = config['color']['height']
out_vid = cv2.VideoWriter(video,
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'),
30, (frame_width, frame_height))
all_records = []
counter = 0
try:
while True:
t00 = time.perf_counter()
try:
color_image, depth_image, meta = get_frames(
pipeline, t265_pipeline, process_modules, filters, config)
except RuntimeError:
# This only gets thrown when in playback mode from a recoded file when frames "run out"
logging.info("Out of frames")
break
t0 = time.perf_counter()
if color_image is None or not valid_frames(
color_image, depth_image, **
config['polygon']['frameskip']):
logging.debug("Invalid Frames")
continue
t1 = time.perf_counter()
counter += 1
# if counter < 10:
# continue
try:
# Get 6DOF Pose at appropriate timestamp
if config['tracking']['enabled']:
euler_t265 = get_pose_matrix(meta['ts'])
# logging.info('euler_t265: %r', euler_t265)
if config['show_polygon']:
# planes, obstacles, geometric_planes, timings, o3d_mesh = get_polygon(depth_image, config, ll_objects, **meta)
planes, obstacles, geometric_planes, timings = get_polygon(
depth_image, config, ll_objects, **meta)
timings['t_get_frames'] = (t0 - t00) * 1000
timings['t_check_frames'] = (t1 - t0) * 1000
all_records.append(timings)
curb_height = analyze_planes(geometric_planes)
ser.write(("{:.2f}".format(curb_height) + "\n").encode())
# Plot polygon in rgb frame
plot_planes_and_obstacles(planes, obstacles, proj_mat,
None, color_image, config)
# Show images
if config.get("show_images"):
# Convert to open cv image types (BGR)
color_image_cv, depth_image_cv = colorize_images_open_cv(
color_image, depth_image, config)
# Stack both images horizontally
images = np.hstack((color_image_cv, depth_image_cv))
cv2.imshow('RealSense Color/Depth (Aligned)', images)
if video:
out_vid.write(images)
res = cv2.waitKey(1)
if res == ord('p'):
uid = uuid.uuid4()
logging.info("Saving Picture: {}".format(uid))
cv2.imwrite(
path.join(PICS_DIR, "{}_color.jpg".format(uid)),
color_image_cv)
cv2.imwrite(
path.join(PICS_DIR, "{}_stack.jpg".format(uid)),
images)
if res == ord('m'):
plt.imshow(
np.asarray(ll_objects['ico'].image_to_vertex_idx))
plt.show()
plt.imshow(np.asarray(ll_objects['ico'].mask))
plt.show()
plt.imshow(np.asarray(ll_objects['ico'].image))
plt.show()
import ipdb
ipdb.set_trace()
to_save_frames = config['save'].get('frames')
if config['playback'][
'enabled'] and to_save_frames is not None and counter in to_save_frames:
logging.info("Saving Picture: {}".format(counter))
cv2.imwrite(
path.join(PICS_DIR,
"{}_color.jpg".format(counter)),
color_image_cv)
cv2.imwrite(
path.join(PICS_DIR,
"{}_stack.jpg".format(counter)), images)
# logging.info(f"Frame %d; Get Frames: %.2f; Check Valid Frame: %.2f; Laplacian: %.2f; Bilateral: %.2f; Mesh: %.2f; FastGA: %.2f; Plane/Poly: %.2f; Filtering: %.2f; Geometric Planes: %.2f; Curb Height: %.2f",
# counter, timings['t_get_frames'], timings['t_check_frames'], timings['t_laplacian'], timings['t_bilateral'], timings['t_mesh'], timings['t_fastga_total'],
# timings['t_polylidar_planepoly'], timings['t_polylidar_filter'], timings['t_geometric_planes'] curb_height)
logging.info(f"Curb Height: %.2f", curb_height)
except Exception as e:
logging.exception("Error!")
finally:
pipeline.stop()
if video is not None:
out_vid.release()
cv2.destroyAllWindows()
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():
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 capture(config, video=None):
# Configure streams
pipeline, process_modules, filters, proj_mat, t265_device = create_pipeline(config)
t265_pipeline = t265_device['pipeline']
logging.info("Pipeline Created")
# Long lived objects. These are the object that hold all the algorithms for surface exraction.
# They need to be long lived (objects) because they hold state (thread scheduler, image datastructures, etc.)
ll_objects = dict()
ll_objects['pl'] = Polylidar3D(**config['polylidar'])
ll_objects['ga'] = GaussianAccumulatorS2(level=config['fastga']['level'])
ll_objects['ico'] = IcoCharts(level=config['fastga']['level'])
if video:
frame_width = config['color']['width'] * 2
frame_height = config['color']['height']
out_vid = cv2.VideoWriter(video, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 30, (frame_width, frame_height))
# Create Homogenous Transform from sensor frame to wheel chair frame
sensor_mount_frame = config['frames']['sensor_mount']
sensor_frame = config['frames']['sensor']
sensor_to_wheel_chair_transform = create_transform(np.array(sensor_mount_frame['translation']), sensor_mount_frame['rotation']) \
@ create_transform(sensor_frame['translation'], sensor_frame['rotation'])
# print(sensor_to_wheel_chair_transform)
# sys.exit()
all_records = []
counter = 0
try:
while True:
t00 = time.perf_counter()
try:
color_image, depth_image, meta = get_frames(pipeline, t265_pipeline, process_modules, filters, config)
except RuntimeError:
# This only gets thrown when in playback mode from a recoded file when frames "run out"
logging.info("Out of frames")
break
t0 = time.perf_counter()
if color_image is None or not valid_frames(color_image, depth_image, **config['polygon']['frameskip']):
logging.debug("Invalid Frames")
continue
t1 = time.perf_counter()
counter += 1
if counter < 1:
continue
try:
# Get 6DOF Pose at appropriate timestamp
if config['tracking']['enabled']:
euler_t265 = get_pose_matrix(meta['ts'])
logging.info('euler_t265: %r', euler_t265)
# flag to write results
have_results = False
if config['show_polygon']:
# planes, obstacles, geometric_planes, timings, o3d_mesh = get_polygon(depth_image, config, ll_objects, **meta)
planes, obstacles, geometric_planes, timings = get_polygon(depth_image, config, ll_objects, **meta)
timings['t_get_frames'] = (t0 - t00) * 1000
timings['t_check_frames'] = (t1 - t0) * 1000
all_records.append(timings)
curb_height, first_plane, second_plane = analyze_planes_updated(geometric_planes)
fname = config['playback']['file'].split('/')[1].split('.')[0]
# dump(dict(first_plane=first_plane, second_plane=second_plane), f"data/scratch_test/planes_{fname}_{counter:04}.joblib")
# curb height must be greater than 2 cm and first_plane must have been found
if curb_height > 0.02 and first_plane is not None:
top_plane = choose_plane(first_plane, second_plane)
top_points, top_normal = top_plane['all_points'], top_plane['normal_ransac']
filtered_top_points = filter_points(top_points) # <100 us
top_points_2d, height, all_fit_lines, best_fit_lines = extract_lines_wrapper(filtered_top_points, top_normal, return_only_one_line=True, **config['linefitting'])
if best_fit_lines:
#If there are two lines only choose the first one
platform_center_sensor_frame = best_fit_lines[0]['hplane_point']
platform_normal_sensor_frame = best_fit_lines[0]['hplane_normal']
# print(platform_center_sensor_frame, platform_normal_sensor_frame)
result = get_turning_manuever(platform_center_sensor_frame, platform_normal_sensor_frame, \
sensor_to_wheel_chair_transform, poi_offset=config.get('poi_offset', 0.7), debug=False)
plt.show()
orthog_dist = result['ortho_dist_platform']
distance_of_interest = result['dist_poi']
orientation = result['beta']
initial_turn = result['first_turn']
final_turn= result['second_turn']
logging.info("Frame #: %s, Orthogonal Distance to Platform: %.02f m, Orientation: %0.1f Degrees, Distance to POI: %.02f m, " \
"First Turn: %.01f degrees, Second Turn: %.01f degrees",
counter, orthog_dist, orientation, distance_of_interest, initial_turn, final_turn)
plot_points(best_fit_lines[0]['square_points'], proj_mat, color_image, config)
# plot_points(best_fit_lines[0]['points_3d_orig'], proj_mat, color_image, config)
if len(best_fit_lines) > 2:
plot_points(best_fit_lines[1]['square_points'], proj_mat, color_image, config)
have_results = True
else:
logging.warning("Line Detector Failed")
else:
logging.warning("Couldn't find the street and sidewalk surface")
# sys.exit()
# Plot polygon in rgb frame
plot_planes_and_obstacles(planes, obstacles, proj_mat, None, color_image, config)
# import ipdb; ipdb.set_trace()
# Show images
if config.get("show_images"):
# Convert to open cv image types (BGR)
color_image_cv, depth_image_cv = colorize_images_open_cv(color_image, depth_image, config)
# Stack both images horizontally
images = np.hstack((color_image_cv, depth_image_cv))
if have_results:
cv2.putText(images,'Curb Height: '"{:.2f}" 'm'.format(curb_height), (20,360), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1, cv2.LINE_AA)
cv2.putText(images,'Orthogonal Distance: '"{:.2f}" 'm'.format(orthog_dist), (20,380), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1, cv2.LINE_AA)
cv2.putText(images,'Distance to Point of Interest: '"{:.2f}" 'm'.format(distance_of_interest), (20,400), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1, cv2.LINE_AA)
cv2.putText(images,'Initial Turn: '"{:.2f}" 'deg'.format(initial_turn), (20,420), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1, cv2.LINE_AA)
cv2.putText(images,'Orientation: '"{:.2f}" 'deg'.format(orientation), (20,440), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1, cv2.LINE_AA)
cv2.putText(images,'Angle for final turn: '"{:.2f}" 'deg'.format(final_turn), (20,460), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1, cv2.LINE_AA)
cv2.imshow('RealSense Color/Depth (Aligned)', images)
# visualize_2d(top_points_2d, top_points_2d, all_fit_lines, best_fit_lines)
if video:
out_vid.write(images)
res = cv2.waitKey(1)
if res == ord('p'):
uid = uuid.uuid4()
logging.info("Saving Picture: {}".format(uid))
cv2.imwrite(path.join(PICS_DIR, "{}_color.jpg".format(uid)), color_image_cv)
cv2.imwrite(path.join(PICS_DIR, "{}_stack.jpg".format(uid)), images)
if res == ord('m'):
plt.imshow(np.asarray(ll_objects['ico'].image_to_vertex_idx))
plt.show()
plt.imshow(np.asarray(ll_objects['ico'].mask))
plt.show()
plt.imshow(np.asarray(ll_objects['ico'].image))
plt.show()
to_save_frames = config['save'].get('frames')
if config['playback']['enabled'] and to_save_frames is not None and counter in to_save_frames:
logging.info("Saving Picture: {}".format(counter))
cv2.imwrite(path.join(PICS_DIR, "{}_color.jpg".format(counter)), color_image_cv)
cv2.imwrite(path.join(PICS_DIR, "{}_stack.jpg".format(counter)), images)
# logging.info(f"Frame %d; Get Frames: %.2f; Check Valid Frame: %.2f; Laplacian: %.2f; Bilateral: %.2f; Mesh: %.2f; FastGA: %.2f; Plane/Poly: %.2f; Filtering: %.2f; Geometric Planes: %.2f",
# counter, timings['t_get_frames'], timings['t_check_frames'], timings['t_laplacian'], timings['t_bilateral'], timings['t_mesh'], timings['t_fastga_total'],
# timings['t_polylidar_planepoly'], timings['t_polylidar_filter'], timings['t_geometric_planes'])
logging.info(f"Curb Height: %.2f", curb_height)
except Exception as e:
logging.exception("Error!")
finally:
pipeline.stop()
if video is not None:
out_vid.release()
cv2.destroyAllWindows()
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 capture(config, video=None):
# Configure streams
pipeline, process_modules, filters, proj_mat, t265_device = create_pipeline(
config)
t265_pipeline = t265_device['pipeline']
logging.info("Pipeline Created")
# Long lived objects. These are the object that hold all the algorithms for surface exraction.
# They need to be long lived (objects) because they hold state (thread scheduler, image datastructures, etc.)
ll_objects = dict()
ll_objects['pl'] = Polylidar3D(**config['polylidar'])
ll_objects['ga'] = GaussianAccumulatorS2(level=config['fastga']['level'])
ll_objects['ico'] = IcoCharts(level=config['fastga']['level'])
if video:
frame_width = config['color']['width'] * 2
frame_height = config['color']['height']
out_vid = cv2.VideoWriter(video,
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'),
30, (frame_width, frame_height))
all_records = []
counter = 0
try:
while True:
t00 = time.perf_counter()
try:
color_image, depth_image, meta = get_frames(
pipeline, t265_pipeline, process_modules, filters, config)
except RuntimeError:
# This only gets thrown when in playback mode from a recoded file when frames "run out"
logging.info("Out of frames")
break
t0 = time.perf_counter()
if color_image is None or not valid_frames(
color_image, depth_image, **
config['polygon']['frameskip']):
logging.debug("Invalid Frames")
continue
t1 = time.perf_counter()
counter += 1
if counter < 1:
continue
try:
# Get 6DOF Pose at appropriate timestamp
if config['tracking']['enabled']:
euler_t265 = get_pose_matrix(meta['ts'])
logging.info('euler_t265: %r', euler_t265)
if config['show_polygon']:
# planes, obstacles, geometric_planes, timings, o3d_mesh = get_polygon(depth_image, config, ll_objects, **meta)
planes, obstacles, geometric_planes, timings = get_polygon(
depth_image, config, ll_objects, **meta)
timings['t_get_frames'] = (t0 - t00) * 1000
timings['t_check_frames'] = (t1 - t0) * 1000
all_records.append(timings)
curb_height, first_plane, second_plane = analyze_planes(
geometric_planes)
# curb height must be greater than 2 cm and first_plane must have been found
if curb_height > 0.02 and first_plane is not None:
square_points, normal_svm, center = hplane(
first_plane, second_plane)
dist, theta = get_theta_and_distance(
normal_svm, center, first_plane['normal_ransac'])
logging.info(
"Frame #: %s, Distance: %.02f meters, Theta: %.01f degrees",
counter, dist, theta)
plot_points(square_points, proj_mat, color_image,
config)
# dump(dict(first_plane=first_plane, second_plane=second_plane), 'data/planes.joblib')
else:
logging.warning(
"Couldn't find the street and sidewalk surface")
# Send arduino curb height, distance to curb, and Angle to the curb
# ser.write(("{:.2f}".format(curb_height)+"\n").encode())
# ser.write(("{:.2f}".format(dist)+"\n").encode())
# ser.write(("{:.2f}".format(theta)+"\n").encode())
# Send RPi through Socket curb height, distance to curb, and Angle to the curb
s = socket.socket()
host = "169.254.41.103" #This is your Server IP!
port = 2345
s.connect((host, port))
data = struct.pack('!d', ("{:.2f}".format(curb_height)))
s.send(data)
rece = s.recv(1024)
print("Received", rece)
s.close()
# sys.exit()
# Plot polygon in rgb frame
plot_planes_and_obstacles(planes, obstacles, proj_mat,
None, color_image, config)
# import ipdb; ipdb.set_trace()
# Show images
if config.get("show_images"):
# Convert to open cv image types (BGR)
color_image_cv, depth_image_cv = colorize_images_open_cv(
color_image, depth_image, config)
# Stack both images horizontally
images = np.hstack((color_image_cv, depth_image_cv))
cv2.putText(
images, 'Curb Height: '
"{:.2f}"
'm'.format(curb_height), (10, 200),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1,
cv2.LINE_AA)
cv2.putText(
images, 'Distance from Curb: '
"{:.2f}"
'm'.format(dist), (10, 215), cv2.FONT_HERSHEY_SIMPLEX,
0.5, (0, 255, 0), 1, cv2.LINE_AA)
cv2.putText(
images, 'Angle to the Curb: '
"{:.2f}"
'deg'.format(theta), (10, 230),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1,
cv2.LINE_AA)
cv2.imshow('RealSense Color/Depth (Aligned)', images)
if video:
out_vid.write(images)
res = cv2.waitKey(1)
if res == ord('p'):
uid = uuid.uuid4()
logging.info("Saving Picture: {}".format(uid))
cv2.imwrite(
path.join(PICS_DIR, "{}_color.jpg".format(uid)),
color_image_cv)
cv2.imwrite(
path.join(PICS_DIR, "{}_stack.jpg".format(uid)),
images)
if res == ord('m'):
plt.imshow(
np.asarray(ll_objects['ico'].image_to_vertex_idx))
plt.show()
plt.imshow(np.asarray(ll_objects['ico'].mask))
plt.show()
plt.imshow(np.asarray(ll_objects['ico'].image))
plt.show()
# import ipdb; ipdb.set_trace()
to_save_frames = config['save'].get('frames')
if config['playback'][
'enabled'] and to_save_frames is not None and counter in to_save_frames:
logging.info("Saving Picture: {}".format(counter))
cv2.imwrite(
path.join(PICS_DIR,
"{}_color.jpg".format(counter)),
color_image_cv)
cv2.imwrite(
path.join(PICS_DIR,
"{}_stack.jpg".format(counter)), images)
# logging.info(f"Frame %d; Get Frames: %.2f; Check Valid Frame: %.2f; Laplacian: %.2f; Bilateral: %.2f; Mesh: %.2f; FastGA: %.2f; Plane/Poly: %.2f; Filtering: %.2f; Geometric Planes: %.2f; Curb Height: %.2f",
# counter, timings['t_get_frames'], timings['t_check_frames'], timings['t_laplacian'], timings['t_bilateral'], timings['t_mesh'], timings['t_fastga_total'],
# timings['t_polylidar_planepoly'], timings['t_polylidar_filter'], timings['t_geometric_planes'] curb_height)
logging.info(f"Curb Height: %.2f", curb_height)
except Exception as e:
logging.exception("Error!")
finally:
pipeline.stop()
if video is not None:
out_vid.release()
cv2.destroyAllWindows()
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 evaluate_with_params(param_set,
param_index,
variance,
counter=None,
data='train'):
"""Evaluates polylidar against a set of parameters and writes results to a csv file
Arguments:
param_set {List[Dict]} -- A list of parameters dictionaries
param_index {int} -- A unique index of the param_set if it was split from a master list
variance {int} -- The level of variance of files to pull from SynPEB
Keyword Arguments:
counter {Value} -- Multiprocessing Value for incementing progress in safe manner (default: {None})
data {str} -- To pull from training or testing (default: {'train'})
"""
from polylidar_plane_benchmark.utility.helper import (
convert_planes_to_classified_point_cloud, load_pcd_file,
predict_loops_laplacian, extract_all_dominant_plane_normals,
extract_planes_and_polygons_from_mesh)
from polylidar_plane_benchmark.utility.evaluate import evaluate
from polylidar_plane_benchmark.utility.helper_mesh import create_meshes_cuda
from polylidar import Polylidar3D
from fastga import GaussianAccumulatorS2, IcoCharts
all_fpaths = get_fpaths(variance, data=data)
optimized = data == 'test'
# Create Long Lived Objects Only Once
ga = GaussianAccumulatorS2(
level=level_default) # Fast Gaussian Accumulator
ico_chart = IcoCharts(
level=level_default
) # Used for unwrapping S2 to Image for peak detection
pl = Polylidar3D(
**polylidar_kwargs_default) # Region Growing and Polygons Extraction
# logger_train.warn("Working on variance %r, param index %r with %r elements", variance, param_index, len(param_set))
csv_fpath = get_csv_fpath(variance, param_index, data=data)
with open(csv_fpath, 'w', newline='') as csv_file:
fieldnames = [
'variance', 'fname', 'tcomp', 'kernel_size', 'loops_bilateral',
'loops_laplacian', 'sigma_angle', 'norm_thresh_min', 'stride',
'min_triangles', 'predict_loops_laplacian', 'n_gt', 'n_ms_all',
'f_weighted_corr_seg', 'f_corr_seg', 'n_corr_seg', 'n_over_seg',
'n_under_seg', 'n_missed_seg', 'n_noise_seg', 'rmse', 'laplacian',
'bilateral', 'mesh', 'fastga_total', 'fastga_integrate',
'fastga_peak', 'polylidar'
]
writer = csv.DictWriter(csv_file, fieldnames=fieldnames)
writer.writeheader()
try:
for fpath in all_fpaths:
fname = pathlib.Path(fpath).name
logger_train.info("Working on file %s", fpath)
mesh_kwargs_previous = dict()
tri_mesh = None
timings_mesh = None
prev_stride = 0
pc_raw = None
pc_image = None
for params in param_set:
stride = params.get('stride', 2)
if stride != prev_stride:
pc_raw, pc_image = load_pcd_file(str(fpath),
stride=stride)
prev_stride = stride
else:
logger_train.debug(
"Reusing previously loaded point cloud")
# Predict laplacian loops needed by noisiness of point cloud
if params.get('predict_loops_laplacian'):
params['loops_laplacian'] = predict_loops_laplacian(
pc_image, stride)
logger_train.info("Set laplacian loops to :%r",
params['loops_laplacian'])
# Update Polylidar Kwargs
pl.norm_thresh = params['norm_thresh_min']
pl.norm_thresh_min = params['norm_thresh_min']
pl.min_triangles = params.get('min_triangles', 250)
t1 = time.perf_counter()
mesh_kwargs = {
k: params[k]
for k in ('loops_laplacian', 'kernel_size',
'loops_bilateral', 'sigma_angle')
}
# Create Smoothed TriMesh
if mesh_kwargs_previous != mesh_kwargs:
tri_mesh, mesh_timings = create_meshes_cuda(
pc_image, **mesh_kwargs)
mesh_kwargs_previous = mesh_kwargs
else:
logger_train.debug("Reusing previously created mesh!")
# Extract Dominant Plane Peaks
avg_peaks, _, _, _, fastga_timings = extract_all_dominant_plane_normals(
tri_mesh,
level=level_default,
with_o3d=False,
ga_=ga,
ico_chart_=ico_chart)
# Extact Planes and Polygons
try:
all_planes, all_polygons, all_poly_lines, polylidar_timings = extract_planes_and_polygons_from_mesh(
tri_mesh,
avg_peaks,
filter_polygons=False,
pl_=pl,
optimized=optimized)
except Exception:
logger_train.exception(
"Error in Polylidar, File: %s, Variance: %d, Param Index: %d, Params: %r",
fname, variance, param_index, params)
all_planes = []
all_polygons = []
all_poly_lines = []
polylidar_timings = dict()
logger_train.exception(
"Error in Polylidar, Params: %r", params)
# Aggregate timings
all_timings = dict(**mesh_timings, **fastga_timings,
**polylidar_timings)
# Convert planes to format used for evaluation
all_planes_classified = convert_planes_to_classified_point_cloud(
all_planes, tri_mesh, avg_peaks)
# Evaluate the results. This actually takes the longest amount of time!
misc = dict(fname=fname,
variance=variance,
tcomp=0.80,
**params)
results_080, auxiliary_080 = evaluate(
pc_image, all_planes_classified, tcomp=0.80, misc=misc)
# print(results)
logger_train.info("Finished %r", params)
full_record_080 = dict(**params,
**results_080,
**all_timings,
fname=fname,
variance=variance,
tcomp=0.80)
# Save the record
writer.writerow(full_record_080)
csv_file.flush()
t2 = time.perf_counter()
elapsed = (t2 - t1) * 1000
logger_train.info("One Param Set Took: %.1f", elapsed)
# Don't forget to clear the GA
ga.clear_count()
if counter is not None:
with counter.get_lock():
counter.value += 1
except Exception as e:
logger_train.exception("Something went wrong")
def evaluate_with_params_visualize(params):
"""Similar to above but just for one set of parameters. Used for debugging and visualization
Arguments:
params {dict} -- Dict of hyperparameters
"""
from polylidar_plane_benchmark.utility.helper import (
convert_planes_to_classified_point_cloud, load_pcd_file, paint_planes,
predict_loops_laplacian, extract_all_dominant_plane_normals,
extract_planes_and_polygons_from_mesh)
from polylidar_plane_benchmark.utility.evaluate import evaluate, evaluate_incorrect
from polylidar_plane_benchmark.utility.helper_mesh import create_meshes_cuda
from polylidar_plane_benchmark.utility.o3d_util import create_open_3d_pcd, plot_meshes, create_open_3d_mesh_from_tri_mesh, mark_invalid_planes
from polylidar import Polylidar3D
from fastga import GaussianAccumulatorS2, IcoCharts
import open3d as o3d
logger.setLevel(logging.INFO)
variance = params['variance']
all_fpaths = get_fpaths(params['variance'])
fpaths = [fpath for fpath in all_fpaths if params['fname'] in fpath]
fpath = fpaths[0]
# Create Long Lived Objects Only Once
ga = GaussianAccumulatorS2(
level=level_default) # Fast Gaussian Accumulator
ico_chart = IcoCharts(
level=level_default
) # Used for unwrapping S2 to Image for peak detection
pl = Polylidar3D(
**polylidar_kwargs_default) # Region Growing and Polygons Extraction
fname = pathlib.Path(fpath).name
logger_train.info("Working on file %s", fpath)
stride = params.get('stride', 2)
pc_raw, pc_image = load_pcd_file(fpath, stride=stride)
pc_points = np.ascontiguousarray(pc_raw[:, :3])
# Create Open3D point cloud
cmap = cc.cm.glasbey_bw
pcd_raw = create_open_3d_pcd(pc_raw[:, :3], pc_raw[:, 3], cmap=cmap)
pl.norm_thresh = params['norm_thresh_min']
pl.norm_thresh_min = params['norm_thresh_min']
pl.min_triangles = params.get('min_triangles', 250)
# Predict laplacian loops needed by noisiness of point cloud
if params.get('predict_loops_laplacian'):
params['loops_laplacian'] = predict_loops_laplacian(pc_image, stride)
logger_train.info("Set laplacian loops to :%r",
params['loops_laplacian'])
t1 = time.perf_counter()
mesh_kwargs = {
k: params[k]
for k in ('loops_laplacian', 'kernel_size', 'loops_bilateral',
'sigma_angle')
}
tri_mesh, mesh_timings = create_meshes_cuda(pc_image, **mesh_kwargs)
mesh_kwargs_previous = mesh_kwargs
# Extract Dominant Plane Peaks
avg_peaks, pcd_all_peaks, arrow_avg_peaks, colored_icosahedron, fastga_timings = extract_all_dominant_plane_normals(
tri_mesh,
level=level_default,
with_o3d=True,
ga_=ga,
ico_chart_=ico_chart)
# Extact Planes and Polygons
try:
all_planes, all_polygons, all_poly_lines, polylidar_timings = extract_planes_and_polygons_from_mesh(
tri_mesh,
avg_peaks,
filter_polygons=False,
pl_=pl,
optimized=False)
except Exception:
all_planes = []
all_polygons = []
all_poly_lines = []
polylidar_timings = dict()
logger_train.exception("Error in Polylidar, Params: %r", params)
# Aggregate timings
all_timings = dict(**mesh_timings, **fastga_timings, **polylidar_timings)
# Convert planes to format used for evaluation
all_planes_classified = convert_planes_to_classified_point_cloud(
all_planes, tri_mesh, avg_peaks)
# Evaluate the results. This actually takes the longest amount of time!
misc = dict(**params)
# results_080, auxiliary_080 = evaluate(pc_image, all_planes_classified, tcomp=0.80, misc=misc)
results_080, auxiliary_080 = evaluate(pc_image,
all_planes_classified,
tcomp=params['tcomp'],
misc=misc)
# print(results)
logger_train.info("Finished %r", params)
# create invalid plane markers, green = gt_label_missed, red=ms_labels_noise, blue=gt_label_over_seg, gray=ms_label_under_seg
invalid_plane_markers = mark_invalid_planes(pc_raw, auxiliary_080,
all_planes_classified)
tri_mesh_o3d = create_open_3d_mesh_from_tri_mesh(tri_mesh)
tri_mesh_o3d_painted = paint_planes(all_planes_classified, tri_mesh_o3d)
# plot_meshes([pcd_raw, tri_mesh_o3d], [colored_icosahedron, *arrow_avg_peaks])
plot_meshes([pcd_raw, tri_mesh_o3d_painted, *invalid_plane_markers])
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()
