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 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 intersect_pairs(c, q):
'''
Find intersections of each sequential pair of rays
c: ray origin
q: ray direction
'''
N = c.shape[1]
lines = empty((N, 3))
lines[:, 0] = -q[1]
lines[:, 1] = q[0]
lines[:, 2] = -sum_ax(c.T * lines[:, :2], axis=1)
points = empty((N - 1, 2))
for i in range(N - 1):
p = Geom.cross3(lines[i], lines[i + 1])
points[i] = p[:2] / p[2]
return points.T
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
