def normals_label(tans, qs, keep, labels, s, local_normals, labels_frame):
label = 1
for i in range(len(keep) - 1):
j = keep[i]
k = s + i
if labels[j] != 0:
labels_frame[k] = label
else:
labels_frame[k] = 0
normal = Geom.cross3(tans[j], qs[j])
mag = Geom.norm3(normal)
if mag == 0:
local_normals[k] = 1, 0, 0
labels_frame[k] = 0
else:
normal /= mag
local_normals[k] = normal
if labels[j + 1] != labels[j]:
label += 1
j = keep[-1]
normal = Geom.cross3(tans[j], qs[j])
normal /= Geom.norm3(normal)
local_normals[k] = normal
local_normals[s + len(keep) - 1] = normal
labels_frame[s + len(keep) - 1] = 0
def render_frame(im, object_mesh, occlusion_mesh, poly_mesh, R, t, cam, light=None, color=None, poly_color=None):
if light is None:
# lazi, lalt = np.deg2rad(30), np.deg2rad(60)
lazi, lalt = np.deg2rad(30), np.deg2rad(45)
light = np.r_[cos(lazi)*cos(lalt), sin(lazi)*cos(lalt), sin(lalt)].astype(np.float32)
light /= Geom.norm3(light)
light = dot(R.T, light).astype(np.float64)
# light = light.astype(np.float64)
# raise Exception("TODO: Light")
view = -dot(R.T, t)
pts_obj = object_mesh.project_verts(R, t, cam)
obj_map = np.full(im.shape[:2], np.inf, np.float32)
render_depth(obj_map, pts_obj, object_mesh)
# --- Buffer occlusion lines ----
if color is None:
color = np.r_[0, 155, 255]
pts_occl = occlusion_mesh.project_verts(R, t, cam)
norm_proj = dot(occlusion_mesh.normals, R.T)
if len(occlusion_mesh.edges) == 0:
line_buf = buffer_points(pts_occl, color, alpha=0.1, size=3)
else:
line_buf = buffer_lines(pts_occl, norm_proj, occlusion_mesh, color, view, light, alpha=0.1, shade_method=NORMAL)
if poly_mesh is not None:
if poly_color is None:
poly_color = np.r_[255, 166, 0]
pts_poly = poly_mesh.project_verts(R, t, cam)
norm_poly = dot(poly_mesh.normals, R.T)
poly_buf = buffer_lines(pts_poly, norm_poly, poly_mesh, poly_color, view, light, linewidth=4, shade_method=FLAT)
poly_buf[:, 2] = 0.001
else:
poly_buf = np.empty((0, line_buf.shape[1]), line_buf.dtype)
# Make buffer out of object depth map, and append to line buffer
I, J = np.where(obj_map != np.inf)
depths = 1.02*obj_map[I, J]
object_buf = empty((len(I), line_buf.shape[1]), line_buf.dtype)
object_buf[:, 0] = I
object_buf[:, 1] = J
object_buf[:, 2] = depths
object_buf[:, 3] = 0.4
object_buf[:, 4:] = 255, 255, 255
final_buf = np.vstack((line_buf, poly_buf, object_buf))
render_buffer(im, final_buf, bg=255)
def normals_from_ray_pairs(Qs):
'''
Each sequential ray is treated as bases for normals
'''
N = Qs.shape[0]
normals = np.empty_like(Qs)
for i in range(0, N - 1):
n = Geom.cross3(Qs[i + 1], Qs[i])
mag = Geom.norm3(n)
for j in range(3):
normals[i, j] = n[j] / mag
normals[-1] = normals[-2]
return normals
def smooth_polys(verts, starts, sizes, support=10):
poly_dir = np.empty_like(verts)
S = len(starts)
for p in range(S):
start = starts[p]
size = sizes[p]
poly = verts[start:start + size]
N = len(poly)
for i in range(N):
s = max(0, i - support)
e = min(N - 1, i + support)
v = poly[e] - poly[s]
v /= Geom.norm3(v)
poly_dir[start + i] = v
return poly_dir
def buffer_lines(pts, norms_proj, mesh, color, view, light, alpha=1.0, shade_method=PHONG, linewidth=1):
'''
Use the depth buffer, dbuf, to figure out whether the object needs to be "washed out"
when drawing.
Use the normal information to make clear the normal direction of the line.
'''
edges = mesh.edges
normals = mesh.normals
E = edges.shape[0]
# Output: [i, j, d, alpha, r, g, b]
size = 1024
P = 7
out = empty((size, P), np.float32)
N = 0
for e in range(E):
edge = edges[e]
x1, y1, z1 = pts[edge[0]]
x2, y2, z2 = pts[edge[1]]
view_local = (view - mesh.verts[edge[0]]).astype(np.float64)
# n1, n2 = normals[edge[0]], normals[edge[1]]
normal = normals[e].astype(np.float64)
normal_l = norms_proj[e].astype(np.float64)
normal_l /= Geom.norm3(normal_l)
# light_angle = abs(dot(normal, light))*1.5 + 0.3
# print(light_angle)
I, J, a = Drawing.line_aa(x1, y1, x2, y2, linewidth)
M = len(I)
depths = np.linspace(z1, z2, M)
# Resize if necessary
resize = False
while size < N + M:
size *= 2
resize = True
if resize:
newout = empty((size, P), np.float32)
newout[:N] = out[:N]
out = newout
if shade_method == PHONG:
out_color = phong_shade(color, light, normal, view_local)
elif shade_method == NORMAL:
out_color = normal_shade(light, normal, normal_l)
else:
out_color = color.astype(np.float64)
# Copy it in
for i, n in enumerate(range(N, N + M)):
out[n, 0] = I[i]
out[n, 1] = J[i]
out[n, 2] = depths[i]
out[n, 3] = a[i] * alpha
out[n, 4:] = out_color
N += M
return out[:N]
def detect_loop(ray_inds, Cs, Qs, pluckers, planes, labels, frames, raygrid,
config, score, total, curv_mean, m2, depths_out, radii_out,
inlier_out, edge_angles_out, neighbors, status):
'''
score: Output, integer array classifying the type of each segment
'''
# Each valid segment is labelled with a non-zero label
# tim = Util.LoopTimer()
# loop_i = 0
for ray_ind in ray_inds:
# tim.loop_start(loop_i)
# loop_i += 1
if labels[ray_ind] == 0:
status[ray_ind] = -1
score[ray_ind] = 0
continue
# First find other segments that are nearby, and might overlap
# The RayVoxel.Grid class offers very fast lookups at the cost of memory
near_grid = raygrid.rays_near_ind(ray_ind)
# tim.add("RayGrid Search")
if len(near_grid) < 10:
status[ray_ind] = -1
continue
# Filter frame support
near = filter_frames(near_grid, frames, ray_ind, config.frame_support)
# close_frames = np.abs(frames[near_grid] - frames[ray_ind]) <= config.frame_support
# near = near_grid[close_frames]
# tim.add("Filter frames")
# Intersect the shortlisted segments with this segment
# ps, us describe the intersection locations of the other segments'
# start- and end-points.
ps1, us, intersecting = find_intersections(Cs, Qs, pluckers, ray_ind,
planes, near)
# print("Near:", len(near))
# tim.add("Find intersections")
if len(intersecting) < 10:
status[ray_ind] = -1
continue
ray = 0.5 * (Qs[ray_ind] + Qs[ray_ind + 1])
ray /= Geom.norm3(ray)
plane = planes[ray_ind]
normal = plane[:3]
tangent = Geom.cross3(ray, normal)
# Create rotation matrix to orthoganally project intersections
# into plane of this segment. This plane is tangent to the edge
R_tang = empty((2, 3), ps1.dtype)
R_tang[0] = ray
R_tang[1] = tangent
center = Cs[ray_ind]
ps2d = dot(R_tang, (ps1 - center).T)
us2d = dot(R_tang, us.T)
px, py = ps2d[0], ps2d[1]
ux, uy = us2d[0], us2d[1]
# Solve for the intersection with the center of the segment
# [x, 0] = p + t*u
uy[uy == 0] = 1e-10
ts = -py / uy
depths = px + ts * ux
# Project the intersecting segments into the normal plane.
# This plane's normal is roughly aligned with the edge direction.
R_norm = empty((3, 3), ps1.dtype)
R_norm[0] = ray
R_norm[1] = normal
R_norm[2] = tangent
centers = dot(R_norm, (Cs[intersecting] - center).T)
# plane_angles = np.arctan(centers[1]/centers[2])
# Check where start and end points straddles center ray of this segment
crossing = py * (py + uy) < 0
good_depths = (depths > 0.2) & (depths < 1e6)
# close_frames = np.abs(frames[intersecting] - frames[ray_ind]) <= config.frame_support
# in_normal_plane = np.abs(plane_angles) <= config.planar_support
sel = np.where(good_depths & crossing)[0]
# sel = np.where(good_depths & crossing & (close_frames | in_normal_plane))[0]
# tim.add("Intersect filter")
# print("Sel:", len(sel))
# We're looking for clusters of line segments in the tangent plane
# These indicate a high likelihood of a persistent edge
# frames = cloud.frames[ intersecting[sel] ]
# p = ps2d[:,sel]
u = us2d[:, sel]
d = depths[sel]
c_sel = centers[:, sel]
rays = empty((2, len(sel)), centers.dtype)
rays[0] = d - c_sel[0]
rays[1] = -c_sel[1]
rays /= CircleRays.norm_ax(rays, axis=0)
# tim.add("Prepare clustering")
dthresh = config.depth_thresh
# Cluster based on circle space
cluster_inds, radius, depth, dual_centers, dual_rays, inlier_frac = find_cluster_line4(
c_sel, rays, depth_thresh=dthresh, percentile=config.inlier_thresh)
# tim.add("Clustering")
cluster_full = intersecting[sel[cluster_inds]]
M = min(neighbors.shape[1], len(cluster_full))
if inlier_frac > 0.02:
if abs(radius) < config.min_radius:
# Hard edge
status[ray_ind] = 1
score[ray_ind] += len(cluster_full)
else:
status[ray_ind] = 2
score[ray_ind] += len(cluster_full)
# Want to estimate the depths of the rays on either side of the segment
u_cluster = u[:, cluster_inds]
intersect_angles = np.arctan(-u_cluster[0] / u_cluster[1])
# Zero is verticle
alpha = np.median(intersect_angles)
# Find angle between rays
theta = 0.5 * arccos(dot(Qs[ray_ind], Qs[ray_ind + 1]))
d1 = depth * sin(PI / 2 + alpha) / sin(PI / 2 - alpha - theta)
d2 = depth * sin(PI / 2 - alpha) / sin(PI / 2 + alpha - theta)
depths_out[ray_ind, 0] = d1
depths_out[ray_ind, 1] = d2
depths_out[ray_ind, 2] = depth
# depths_out[ray_ind] = depth
radii_out[ray_ind] = radius
inlier_out[ray_ind] = inlier_frac
edge_angles_out[ray_ind] = alpha
ray_c = rays[:, cluster_inds]
cluster_angles = np.abs(np.arctan(ray_c[1] / ray_c[0]))
closest = Util.argsort(cluster_angles)
neighbors[ray_ind, :M] = cluster_full[closest[:M]]
else:
status[ray_ind] = -1
score[ray_ind] = 0
labels[ray_ind] = 0
continue
def cache_segment(Cs, Qs, planes, pluckers, frames, labels, raygrid, inds,
eps):
starts = []
deltas = []
depths = []
sideps = []
sideds = []
ray_frames = []
planar_angles = []
sel_inds = []
j = 0
M = len(inds)
for ind in range(M):
ray_ind = inds[ind]
j += 1
print(j)
print("Checking ray_ind:", ray_ind)
# TODO: Check that this is still working correctly for the triangles
in_cone = raygrid.rays_near_ind(ray_ind)
print("Near: {}/{}".format(len(in_cone), raygrid.n_rays))
if len(in_cone) == 0:
print("Total grid for ray:", raygrid.ray2vox_count[ray_ind])
center = Cs[ray_ind]
ray = Qs[ray_ind:ray_ind + 2].mean(axis=0)
ray /= Geom.norm3(ray)
plane = planes[ray_ind]
normal = plane[:3]
tangent = cross3(ray, normal)
print("Intersect planes")
# TODO: Use Plucker coords to quickly calculate intersection dual ray segments (infinite triangles?)
# Will need labels. Precalc Plucker before (do same as the planes)
# ps -> us, should describe how the other segment intersects this plane
# in terms of it's width
ps, us, intersecting = find_intersections(Cs, Qs, pluckers, ray_ind,
plane, in_cone)
print("Project")
R = empty((3, 3), ps.dtype)
R[0] = ray
R[1] = tangent
R[2] = normal
ps2d = dot(R[:2], (ps - center).T)
us2d = dot(R[:2], us.T)
# Solve for the intersection with the center of the segment
# [x, 0] = p + t*u
us2d[1, us2d[1] == 0] = 1e-10
ts = -ps2d[1] / us2d[1]
ds = ps2d[0] + ts * us2d[0]
# Keep only lines that are more vertical
# vert = np.abs(us2d[0] / (np.abs(us2d[1]) + 1e-12)) < 0.4
crossing = ps2d[1] * (ps2d[1] + us2d[1]) < 0
vert = np.arctan(us2d[0] / us2d[1]) < 0.85
# near = np.abs(ps2d[1]) < eps*ps2d[0]
forward = (ds > 0.2) & (ds < 1e6)
intersecting = in_cone[intersecting]
centers = dot(R, (Cs[intersecting] - center).T)
sidep = empty((2, len(intersecting)), Cs.dtype)
sidep[0] = centers[0]
sidep[1] = centers[2]
rays = empty((3, len(intersecting)), Cs.dtype)
rays[0] = ds - centers[0]
rays[1] = -centers[2]
rays[2] = -centers[1]
# rays = dot(R, Qs[intersecting].T)
sided = np.empty_like(sidep)
sided[0] = rays[0]
sided[1] = rays[1]
# hori = np.abs(sided[1] / sided[0]) < 1.5*eps
sel = np.where(forward & crossing)[0]
ps2d = ps2d[:, sel]
us2d = us2d[:, sel]
starts.append(ps2d)
deltas.append(us2d)
depths.append(ds[sel])
ray_frames.append(frames[intersecting[sel]])
sideps.append(sidep[:, sel])
sideds.append(1.5 * sided[:, sel])
planar_angles.append(np.arctan(centers[1, sel] / centers[2, sel]))
# ray_dot = rays[1, sel] / norm(rays[:,sel], axis=0)
# planar_angles.append( np.arctan(ray_dot) )
sel_inds.append(intersecting[sel])
return starts, deltas, depths, ray_frames, sideps, sideds, sel_inds, planar_angles