from polylidarutil import (plot_points, plot_triangles, plot_triangle_meshes,
get_triangles_from_he, get_plane_triangles,
plot_polygons)
# Load point set that for which delaunator generates invalid convex hulls when using non-robust predicates
# Convex hull should be malformed if polylidar not built with robust predicates
robust_tests = ['robust_1.csv']
for robust_test in robust_tests:
points = load_csv(robust_test)
# Extracts planes and polygons, time
t1 = time.time()
# pick large lmax to get the convex hull, no triangles filtered
delaunay, planes, polygons = extractPlanesAndPolygons(points,
xyThresh=0.0,
alpha=0.0,
lmax=1000.0,
minTriangles=1)
t2 = time.time()
print("Took {:.2f} milliseconds".format((t2 - t1) * 1000))
print(
"If Robust Geoemetric Predicates is NOT activated, you should see a malformed concave hull"
)
print(
"See README.md to activate if desired. ~20% performance penalty when active."
)
print("")
# Plot Data
if points.shape[0] < 100000:
fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1)
ax.scatter(*scale_points(points), c='k', s=0.4)
set_up_axes(fig, ax)
plt.show(block=False)
print(
"Raw Point Cloud of ground floor and two walls (Base Frame). Don't close figure."
)
print("This example will show how to extract all three planes.")
print(
"During this process you will learn the limitations of Polylidar for 3D point clouds and how to work around them."
)
input("Press Enter to Continue: ")
print("")
# extract ground plane
print("Extracting ground plane from raw point cloud. Bottom Plane Extracted")
delaunay, planes, polygons = extractPlanesAndPolygons(points,
**polylidar_kwargs)
triangles = np.asarray(delaunay.triangles).reshape(
int(len(delaunay.triangles) / 3), 3)
plot_planes_3d(points, triangles, planes, ax)
plot_polygons_3d(points, polygons, ax, color=COLOR_PALETTE[0])
plt.show(block=False)
input("Press Enter to Continue: ")
print("")
# Rotate Point Cloud for walls
print(
"The walls must be rotated to be extracted such that their normal is (0,0,1). Rotated Frame."
)
fig2, ax2 = plt.subplots(figsize=(10, 10),
nrows=1,
ncols=1,
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.ascontiguousarray(np.concatenate((plane, box_side, box_top)))
return points
def create_open3d_pc(points):
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
return pcd
points = generate_point_cloud()
t1 = time.perf_counter()
# Create Pseudo 3D Surface Mesh using Delaunay Triangulation and Polylidar
delaunay, planes, polygons = extractPlanesAndPolygons(
points, alpha=0.0, lmax=1.0, minTriangles=20, zThresh=0.2, normThresh=0.98)
t2 = time.perf_counter()
print("Point Size: {}".format(points.shape[0]))
print("2D Delaunay Triangulation and Plane Extraction; Mesh Creation {:.2f} milliseconds".format((t2 - t1) * 1000))
# Visualize
# Create Open3D Mesh
mesh_planes = extract_mesh_planes(points, delaunay, 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()
def generate_polygon(rho, geo='2D', accept_holes=False):
point_cloud = generate_point_list(rho)
if geo == '2D':
# delaunay, planes, polygons = extractPlanesAndPolygons(point_cloud, xyThresh=0.0, alpha=0.0, lmax=0.05, minTriangles=5) # 15
try:
delaunay, planes, polygons = extractPlanesAndPolygons(
point_cloud,
xyThresh=0.0,
alpha=0.0,
lmax=lmax,
minTriangles=5)
except RuntimeError:
return None, None
fig, ax = plt.subplots(figsize=(10, 10), nrows=1, ncols=1)
plot_points(point_cloud, ax)
plot_polygons(polygons, delaunay, point_cloud, ax)
shell_coords = []
hole_coords = []
for poly in polygons:
# shell_coords = [get_point(p_index, point_cloud) for p_index in poly.shell]
shell_coords.append(
[get_point(p_index, point_cloud) for p_index in poly.shell])
try:
poly.holes[0]
except:
hole_coords.append([])
accept_holes = False
if accept_holes:
hole_coords.append([
get_point(p_index, point_cloud)
for p_index in poly.holes[0]
])
# shape = shape.Polygon([ [item[0], item[1]] for item in shell_coords])
# shell_coords = shell_coords[0::2] # FIXME: Talvez necessario
shapes = []
num = 0
rotating = None
for shell in shell_coords:
shell.reverse()
polig = []
for item in shell:
polig.append(Point(item[0], item[1]))
if item[1] < 40.1: rotating = num
num += 1
shapes.append(Polygon(polig))
# hole_coords = hole_coords[0::2]
i = 0
for hole in hole_coords:
if not hole == []:
shape_hole = Polygon(
[Point(item[0], item[1]) for item in hole])
shapes[i] = shapes[i] - shape_hole
i += 1
new_geo = rho.full_geo
'''for shape in shapes:
new_geo = new_geo - shape'''
new_mesh = generate_mesh(new_geo, N)
rho.new_mesh = new_mesh
if rotating is not None:
rotating_countour = np.array(
[[item.x(), item.y()] for item in shapes[rotating].vertices()])
else:
raise NotImplementedError()
class BorderRotating(SubDomain):
def inside(self, x, on_boundary):
global n
global m
line = geometry.LineString(rotating_countour)
point = geometry.Point(x[0], x[1])
# if line.contains(point):
if point.distance(line) < 1e-2 or x[1] <= 40.00001:
n += 1
return True and on_boundary
# return True and on_boundary
else:
m += 1
return False and on_boundary
# return False and on_boundary
class BorderFixed(SubDomain):
def inside(self, x, on_boundary):
return on_boundary and x[1] > 40.000001
print()
print("n = {n}, m = {m}".format(n=n, m=m))
domain = MeshFunction("size_t", new_mesh, 1)
domain.set_all(0)
edge2 = BorderFixed()
edge2.mark(domain, 2)
if rotating is not None:
edge1 = BorderRotating()
edge1.mark(domain, 1)
return new_mesh, domain
def run_test(pcd,
rgbd,
intrinsics,
extrinsics,
bp_alg=dict(radii=[0.02, 0.02]),
poisson=dict(depth=8),
callback=None,
stride=2):
points = np.asarray(pcd.points)
# Create Pseudo 3D Surface Mesh using Delaunay Triangulation and Polylidar
polylidar_kwargs = dict(alpha=0.0,
lmax=0.10,
minTriangles=100,
zThresh=0.03,
normThresh=0.99,
normThreshMin=0.95,
minHoleVertices=6)
t1 = time.perf_counter()
delaunay, planes, polygons = extractPlanesAndPolygons(
points, **polylidar_kwargs)
t2 = time.perf_counter()
all_poly_lines = filter_and_create_open3d_polygons(points, polygons)
triangles = np.asarray(delaunay.triangles).reshape(
int(len(delaunay.triangles) / 3), 3)
mesh_2d_polylidar = extract_mesh_planes(points, triangles, planes,
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 = 'Polylidar2D'
callback(polylidar_alg_name, time_mesh_2d_polylidar, pcd,
mesh_2d_polylidar)
# Uniform Mesh Grid
polylidar_inputs, timings = make_uniform_grid_mesh(
np.asarray(rgbd.depth),
np.ascontiguousarray(intrinsics.intrinsic_matrix),
extrinsics,
stride=stride)
mesh_uniform_grid = create_open_3d_mesh(polylidar_inputs['triangles'],
polylidar_inputs['vertices'])
time_mesh_uniform = timings['mesh_creation']
uniform_alg_name = 'Uniform Grid Mesh'
callback(uniform_alg_name, time_mesh_uniform, pcd, mesh_uniform_grid)
# Polylidar3D with Uniform Mesh Grid
# pickle.dump(polylidar_inputs, open('realsense_mesh.pkl', 'wb'))
vertices = polylidar_inputs['vertices']
triangles = polylidar_inputs['triangles']
halfedges = polylidar_inputs['halfedges']
t1 = time.perf_counter()
planes, polygons = extract_planes_and_polygons_from_mesh(
vertices, triangles, halfedges, **polylidar_kwargs)
t2 = time.perf_counter()
all_poly_lines = filter_and_create_open3d_polygons(vertices, polygons)
triangles = triangles.reshape(int(triangles.shape[0] / 3), 3)
mesh_3d_polylidar = extract_mesh_planes(vertices, triangles, planes)
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 = 'Polylidar with Uniform Grid Mesh'
callback(polylidar_3d_alg_name, time_polylidar3D,
create_open3d_pc(vertices), mesh_3d_polylidar)
# Estimate Point Cloud Normals
t3 = time.perf_counter()
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(
radius=0.10, max_nn=20))
t4 = time.perf_counter()
time_estimate_point_normals = (t4 - t3) * 1000
point_normal_alg_name = 'Point Normal Estimation'
callback(point_normal_alg_name, time_estimate_point_normals, pcd, None)
# Create True 3D Surface Mesh using Ball Pivot Algorithm
radii = o3d.utility.DoubleVector(bp_alg['radii'])
t5 = time.perf_counter()
mesh_ball_pivot = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(
pcd, radii)
prep_mesh(mesh_ball_pivot)
t6 = time.perf_counter()
time_mesh_ball_pivot = (t6 - t5) * 1000
ball_point_alg_name = 'Ball Pivot'
callback(ball_point_alg_name, time_mesh_ball_pivot, pcd, mesh_ball_pivot)
# Create True 3D Surface Mesh using Poisson Reconstruction Algorithm
t7 = time.perf_counter()
mesh_poisson, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(
pcd, **poisson)
vertices_to_remove = densities < np.quantile(densities, 0.1)
mesh_poisson.remove_vertices_by_mask(vertices_to_remove)
t8 = time.perf_counter()
prep_mesh(mesh_poisson)
time_mesh_poisson = (t8 - t7) * 1000
poisson_alg_name = 'Poisson'
callback(poisson_alg_name, time_mesh_poisson, pcd, mesh_poisson)
results = [
dict(alg=polylidar_alg_name,
mesh=mesh_2d_polylidar,
execution_time=time_mesh_2d_polylidar),
dict(alg=point_normal_alg_name,
mesh=None,
execution_time=time_estimate_point_normals),
dict(alg=ball_point_alg_name,
mesh=mesh_ball_pivot,
execution_time=time_mesh_ball_pivot),
dict(alg=poisson_alg_name,
mesh=mesh_poisson,
execution_time=time_mesh_poisson)
]
return results
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))
# Extracts planes and polygons, time
# in practice zThresh would probably be 0.1 with this noise level. Set to large 1.0 to try and force issue of triangle wall climbing.
t1 = time.time()
delaunay, planes, polygons = extractPlanesAndPolygons(points,
xyThresh=0.0,
alpha=0.0,
lmax=1.0,
minTriangles=20,
zThresh=1.0,
normThresh=0.98,
normThreshMin=0.01)
t2 = time.time()
print("Took {:.2f} milliseconds".format((t2 - t1) * 1000))
print("Should see two planes extracted, please rotate.")
print(
"The triangles are climbing the wall because their height is less than zThresh bypassing normal filtering (normThresh)."
)
print(
"This was purposefully done (by settting a very high zThresh) to demonstrate this issue."
)
print("The next plot will fix this by using/setting normThreshMin.")
print(
"It will force ALL triangles to have a MINIMUM planarity no matter their height (dz)."