def draw_pc_poly(client,
poly_client,
ue4_origin=[0, 0, 0],
lifetime=5.0,
polylidar_kwargs=dict(alpha=0.0, lmax=2.0, zThresh=0.1)):
lidar_data = client.getLidarData()
pc = parse_lidar_data(lidar_data)
if pc is None:
return
try:
polygons = extractPolygons(pc, **polylidar_kwargs)
except:
print("Error using Polylidar")
polygons = []
if len(polygons) > 0:
polygon_shapely = convert_to_shapely_polygons(polygons,
pc,
return_first=True,
sort=True)
lr_list = shapely_to_lr_list(polygon_shapely, ue4_origin=ue4_origin)
cmd = DPCommand(lifetime=lifetime,
shell=lr_list['shell'],
holes=lr_list['holes'],
thickness=8.0)
poly_client.draw_polygon(cmd)
def test_building1(benchmark, building1, basic_params):
delaunay, planes, polygons = benchmark(extractPlanesAndPolygons, building1,
**basic_params)
# Basic test to ensure no obvious errors occurred
basic_polylidar_verification(building1, delaunay, planes, polygons)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, building1)
# Test just polygon extraction
polygons = extractPolygons(building1, **basic_params)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, building1)
def verify_all(points, polylidar_kwargs):
delaunay, planes, polygons = extractPlanesAndPolygons(
points, **polylidar_kwargs)
# Basic test to ensure no obvious errors occurred
basic_polylidar_verification(points, delaunay, planes, polygons)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, points)
# Ensure that all polygons are as expected
# Test just polygon extraction
polygons = extractPolygons(points, **polylidar_kwargs)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, points)
def test_hardcase1(hardcase1, hardcase1_params):
# Dont benchmark
delaunay, planes, polygons = extractPlanesAndPolygons(
hardcase1, **hardcase1_params)
# Basic test to ensure no obvious errors occurred
basic_polylidar_verification(hardcase1, delaunay, planes, polygons)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, hardcase1)
# Ensure that all polygons are as expected
# Test just polygon extraction
polygons = extractPolygons(hardcase1, **hardcase1_params)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, hardcase1)
def test_bad_convex_hull(benchmark, bad_convex_hull, bad_convex_hull_params):
delaunay, planes, polygons = benchmark(extractPlanesAndPolygons,
bad_convex_hull,
**bad_convex_hull_params)
# delaunay, planes, polygons = extractPlanesAndPolygons(bad_convex_hull, **bad_convex_hull_params)
# Basic test to ensure no obvious errors occurred
basic_polylidar_verification(bad_convex_hull, delaunay, planes, polygons)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, bad_convex_hull)
# Ensure that all polygons are as expected
# Test just polygon extraction
polygons = extractPolygons(bad_convex_hull, **bad_convex_hull_params)
# Ensure that the polygons returned are valid
verify_all_polygons_are_valid(polygons, bad_convex_hull)
def get_cpp_path(self, start_point):
self.start_tracking()
ground_hegihts = [-11.04, -5.98, 2.29]
for idx, ground_hegiht in enumerate(ground_hegihts[:-1]):
heights = np.asarray(self.pcd.points)[:, 2]
points_idx = np.where(
np.logical_and(heights > ground_hegiht,
heights < ground_hegihts[idx + 1]))[0]
random = np.random.randint(len(points_idx), size=100)
path = np.asarray(self.pcd.points)[points_idx][:, 0:2]
kwargs = dict(alpha=0.0, lmax=1.0)
lmax = get_estimated_lmax(**kwargs)
polygons = extractPolygons(path, alpha=1.0, lmax=1, minTriangles=5)
delaunay = Delaunator(path)
self.print(polygons)
fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1)
# plot points
plot_points(path, ax)
# plot all triangles
#plot_triangles(get_triangles_from_he(delaunay.triangles, path), ax)
# plot mesh triangles
#triangle_meshes = get_plane_triangles(planes, delaunay.triangles, path)
#plot_triangle_meshes(triangle_meshes, ax)
# plot polygons
plot_polygons(polygons, delaunay, points=path, ax=ax)
plt.axis('equal')
plt.show()
self.print(len(points_idx))
self.print(path)
return path
#tree = self.build_RRT_tree(start_point)
path = []
self.print_stats(path)
return path
def get_polygon(points, config, h, w, **kwargs):
"""Extract polygons from point cloud
Arguments:
points {ndarray} -- NX3 numpy array
config {dict} -- Configuration object
h {int} -- height
w {int} -- width
Returns:
tuple -- polygons, rotated downsample points, and rotation matrix
"""
ground_config = config['polygon'][
'ground_normal'] # ground normal parameters
points_config = config['polygon'][
'pointcloud'] # point cloud generation parameters
polylidar_kwargs = config['polygon']['polylidar'] # polylidar parameters
# Get downsample ground patch
ground_patch = [
get_downsampled_patch(points,
h,
w,
patch=ground_config['patch'],
ds=ground_config['ds'])
]
normal = calculate_plane_normal(ground_patch)
# Get downsampled point cloud
points = get_downsampled_patch_advanced(points,
h,
w,
patch=points_config['patch'],
ds=points_config['ds'])
# Rotate cloud to align ground plane normal with Z axis
points_rot, rm = align_vector_to_zaxis(points, normal)
points_rot = np.ascontiguousarray(points_rot)
polygons = extractPolygons(points_rot, **polylidar_kwargs)
return polygons, points_rot, rm