def gen_junction_labels(pth_fn):
pcl, bboxes0 = torch.load(pth_fn)
corners = {}
for obj in bboxes0:
corners_8 = Bbox3D.bboxes_corners(bboxes0[obj], 'Z', False)
corners[obj] = (corners_8[:, Bbox3D._yneg_vs, :] +
corners_8[:, Bbox3D._ypos_vs, :]) / 2.0
#Bbox3D.draw_bboxes(bboxes0[obj], 'Z', False)
#Bbox3D.draw_points(corners[obj].reshape([-1,3]))
#Bbox3D.draw_points_bboxes(corners[obj].reshape([-1,3]), bboxes0[obj], 'Z', False)
connect_num = cal_connection_label(corners[obj])
import pdb
pdb.set_trace() # XXX BREAKPOINT
pass
def preprocess_cfr(ceilings_org, walls_org, obj):
'''
Z is up, Y is thickness
A ceiling is good:
(1) not contains multiple rooms:
cover no other ceilings
(2) >= 3 edge walls
A edge wall of a ceiling:
In three points of the wall cenline: two corners and centroid
at least two are on an edge of ceiling
'''
assert ceilings_org.ndim == walls_org.ndim == 2
dis_threshold = 0.07
if ceilings_org.shape[0] == 0:
return ceilings_org
if walls_org.shape[0] == 0:
return np.zeros(shape=[0, 7], dtype=np.float32)
#Bbox3D.draw_bboxes(walls_org, 'Z', False)
#Bbox3D.draw_bboxes(ceilings_org, 'Z', False)
ceilings = ceilings_org.copy()
ceilings, keep_ids0 = clean_repeat(ceilings)
walls = walls_org.copy()
cn = ceilings.shape[0]
ceilings[:, 2] = 0
walls[:, 2] = 0
walls[:, 5] = 0
ceil_corners0 = Bbox3D.bboxes_corners(ceilings, 'Z')
ceil_corners = np.take(ceil_corners0, Bbox3D._zpos_vs, axis=1)
ceil_corners[:, :, 2] = 0
ceiling_cens = ceilings[:, 0:3]
wall_cenlines = Bbox3D.bboxes_centroid_lines(walls, 'X', 'Z')
good_ceiling_ids0 = []
bad_small_ids = []
for c in range(cn):
# (1)
#print(f'c:{c}')
ceil_c = ceilings[c:c + 1].copy()
ceil_c[:, 3:6] += 0.2
mask_overlap = Bbox3D.points_in_bbox(ceil_corners.reshape([-1, 3]),
ceil_c).reshape([cn, 4])
mask_overlap = mask_overlap.all(1)
num_overlap = mask_overlap.sum() - 1
ol_ids = np.where(mask_overlap)[0]
ol_ids = [i for i in ol_ids if i != c]
#if any_overlap:
# area_ol = np.product(tmp[ol_ids,3:5], axis=1).sum()
# area_c = np.product(ceilings[c,3:5])
# area_ol_rate = area_ol / area_c
# import pdb; pdb.set_trace() # XXX BREAKPOINT
# pass
if Debug and 0:
print(f'ceiling, contain {num_overlap} other celings')
box_show = np.concatenate([walls_org, ceilings_org[c:c + 1]], 0)
Bbox3D.draw_bboxes_mesh(box_show, 'Z', False)
if num_overlap > 1:
continue
#if num_overlap >0:
# bad_small_ids += ol_ids
# (2)
edge_wall_num, winc_state = is_edge_wall_of_ceiling(
wall_cenlines, ceilings[c], walls_org)
if edge_wall_num >= 3 or (edge_wall_num == 2 and
(winc_state == 3).all()):
good_ceiling_ids0.append(c)
#good_ceiling_ids1 = [i for i in good_ceiling_ids0 if i not in bad_small_ids]
good_ceiling_ids1 = keep_ids0[good_ceiling_ids0]
good_ceiling_ids1 = np.array(good_ceiling_ids1).astype(np.int)
rm_num = cn - good_ceiling_ids1.shape[0]
bad_ceiling_ids = np.array(
[i for i in range(cn) if i not in good_ceiling_ids1]).astype(np.int32)
new_ceilings = ceilings_org[good_ceiling_ids1]
if Debug and rm_num > 0:
print(f'{cn} -> {good_ceiling_ids1.shape[0]}')
box_show = np.concatenate([walls_org, ceilings_org[good_ceiling_ids1]],
0)
Bbox3D.draw_bboxes_mesh(box_show, 'Z', False)
if rm_num > 0:
box_show = np.concatenate(
[walls_org, ceilings_org[bad_ceiling_ids]], 0)
Bbox3D.draw_bboxes_mesh(box_show, 'Z', False)
import pdb
pdb.set_trace() # XXX BREAKPOINT
#show_walls_1by1(new_ceilings)
pass
return new_ceilings
def preprocess_doors(doors0, walls, door_thickness=0.18):
'''
doors0:[d,7]
walls:[w,7]
'''
door_n = doors0.shape[0]
if door_n == 0:
return doors0
wall_n = walls.shape[0]
walls_1 = walls.copy()
# (1) Find the coresponding wall of each door
# (1.1) Get the four top lines of each door
door_corners = Bbox3D.bboxes_corners(doors0, 'Z')[:, 0:4, :] # [d,4,3]
door_lines0 = np.take(door_corners, Bbox3D._lines_z0_vids,
axis=1) # [d,4,2,3]
door_lines1 = door_lines0.reshape(-1, 2, 3)
# (1.2) Get the centroid lines of each wall
wall_cen_lines = Bbox3D.bboxes_centroid_lines(walls_1, 'X', 'Z')
# (1.3) Get the intersections_2d between lines and walls
# A door matches a wall means: there are two top lines of the door, which are
# intersected with the wall centroid line.
dw_ints = lines_intersection_2d(door_lines1[:, :, 0:2],
wall_cen_lines[:, :, 0:2], True, True)
dw_ints = dw_ints.reshape(door_n, 4, wall_n, 2) # [d,4,w,2]
tmp0 = (1 - np.isnan(dw_ints)).sum(3) # [d,4,w]
door_mask0 = tmp0.sum(1) == 4
door_matched0 = door_mask0.sum(1).reshape(-1, 1)
# (1.4) Sometimes on door can match two walls. Remove the confused one by:
# The right wall should not contain any corners
dc_in_wall_mask = Bbox3D.points_in_bbox(door_corners.reshape(-1, 3),
walls).reshape(door_n, 4, wall_n)
dc_in_wall_mask = np.any(dc_in_wall_mask, axis=1)
bad_match = (door_matched0 > 1) * dc_in_wall_mask
door_mask = door_mask0 * (1 - bad_match)
door_ids, wall_ids = np.where(door_mask)
success_num = door_ids.shape[0]
# (2) Pick the failed door ids
wall_num_each_door = door_mask.sum(1)
fail_match_door_ids = np.where(wall_num_each_door != 1)[0]
walls_good = walls[wall_ids]
yaws = limit_period(walls_good[:, -1], 0, np.pi / 2)
intersections_2d = []
for i in range(success_num):
door_idx = door_ids[i]
cids = np.where(tmp0[door_idx, :, wall_ids[i]])[0]
ints_i = dw_ints[door_idx, cids, wall_ids[i]]
intersections_2d.append(ints_i.reshape(1, 2, 2))
if len(intersections_2d) == 0:
return np.empty([0, 7])
intersections_2d = np.concatenate(intersections_2d, 0)
doors_centroids_2d = np.mean(intersections_2d, 1)
doors_length = np.linalg.norm(intersections_2d[:, 0] -
intersections_2d[:, 1],
axis=1)
doors_length -= door_thickness * np.sin(yaws * 2)
doors_new = doors0[door_ids].copy()
doors_new[:, 0:2] = doors_centroids_2d
doors_new[:, 3] = doors_length
doors_new[:, 4] = door_thickness
doors_new[:, 6] = walls_good[:, -1]
if DEBUG and fail_match_door_ids.shape[0] > 0:
print(f"fail_match_door_ids:{fail_match_door_ids}")
show_all([doors0[fail_match_door_ids], walls])
#for dids in fail_d_ids:
# fail_w_ids = np.where(door_mask[dids])[0]
# show_all([doors0[dids].reshape(-1,7), walls[fail_w_ids].reshape(-1,7)])
import pdb
pdb.set_trace() # XXX BREAKPOINT
pass
if DEBUG:
show_all([doors0, doors_new, walls])
return doors_new
def add_exta_cam_locations(cam_fn, show=False):
parsed_dir = os.path.dirname(cam_fn)
walls = read_object_bbox(parsed_dir, 'wall')
if walls.shape[0] == 0:
return cam_fn
walls = world2cam_box(walls)
#Bbox3D.draw_bboxes(walls, 'Y', False)
wall_corners = Bbox3D.bboxes_corners(walls, up_axis='Y').reshape([-1, 3])
walls_min = wall_corners.min(0)
walls_max = wall_corners.max(0)
offset = 0.1
step = 2
tmp = np.ceil((walls_max - walls_min + offset * 2) / step) + 1
x_locs = np.arange(0, tmp[0]) * step + walls_min[0] - offset
y_locs = np.arange(0, tmp[1]) * step + walls_min[1] - offset
y_locs = [(walls_min[1] + walls_max[1]) / 2.0]
z_locs = np.arange(0, tmp[2]) * step + walls_min[2] - offset
cam_pos = read_cam_pos(cam_fn)
cam_num = cam_pos.shape[0]
cam_locs0 = cam_pos[:, 0:3]
extra_cams = []
for x in x_locs:
for y in y_locs:
for z in z_locs:
loc = np.array([[x, y, z]])
# check if any wall is close to this loc
dis = np.linalg.norm(loc - wall_corners, axis=1)
dis_min = dis.min()
if dis_min > 2:
# this loc is useles
continue
# check if any cam close to this loc already
dis = np.linalg.norm(loc - cam_locs0, axis=1)
dis_min = dis.min()
if dis_min > 1.5:
cam_new = loc
extra_cams.append(cam_new)
back_cam = np.array([0, 1, 0, cam_pos[0, -3], cam_pos[0, -2], 10])
if len(extra_cams) > 0:
extra_cams = np.concatenate(extra_cams, 0)
cam_pos_new = []
for loc in extra_cams:
cam_forwards = [[1, 0, 0], [-1, 0, 0], [0, 0, 1], [0, 0, -1]]
for i in range(4):
#if i==0:
# valid = loc[0] < walls_min[0]
#elif i==1:
# valid = loc[0] > walls_max[0]
#elif i==2:
# valid = loc[2] > walls_min[2]
#elif i==3:
# valid = loc[2] > walls_max[2]
valid = True
if valid:
forward = np.array(cam_forwards[i])
cam_new_i = np.concatenate([loc, forward, back_cam],
0).reshape([1, 12])
cam_pos_new.append(cam_new_i)
cam_pos_new = np.concatenate(cam_pos_new, 0)
cam_pos_new = np.concatenate([cam_pos_new, cam_pos], 0)
else:
cam_pos_new = cam_pos
cam_fn_new = os.path.dirname(cam_fn) + '/cam'
np.savetxt(cam_fn_new, cam_pos_new, fmt='%.5f')
print(
f'add_exta_cam_locations org cam num: {cam_num}, new cam num: {len(extra_cams)} \n{cam_fn}'
)
save_cam_ply(cam_fn, show)
save_cam_ply(cam_fn_new, show)
return cam_fn_new
def find_close_walls(walls):
'''
walls: [n,7]
'''
show = False
if show:
walls0 = walls.copy()
corners = Bbox3D.bboxes_corners(walls, 'Z') # [n,8,3]
cen_lines_x = Bbox3D.bboxes_centroid_lines(walls, 'X', 'Z') # [n,2,3]
cen_lines_y = Bbox3D.bboxes_centroid_lines(walls, 'Y', 'Z') # [n,2,3]
n = walls.shape[0]
dis_x0 = vertical_dis_points_lines(
corners.reshape([-1, 3])[:, 0:2], cen_lines_x[:, :,
0:2]).reshape([n, 8, n])
dis_x_ave = dis_x0.mean(1) # [n,n]
dis_x_max = dis_x0.max(1) # [n,n]
inside_x_ave = dis_x_ave / (walls[:, 4].reshape([1, n]) * 0.501 + 0.01)
inside_x_max = dis_x_max / (walls[:, 4].reshape([1, n]) * 0.8 + 0.03)
inside_x_mask = (inside_x_ave < 1) * (inside_x_max < 1)
dis_y0 = vertical_dis_points_lines(
corners.reshape([-1, 3])[:, 0:2], cen_lines_y[:, :,
0:2]).reshape([n, 8, n])
dis_y_ave = dis_y0.max(1) # [n,n]
dis_y_max = dis_y0.max(1) # [n,n]
inside_y_ave = dis_y_ave / (walls[:, 3].reshape([1, n]) * 0.515 + 0.03)
inside_y_max = dis_y_max / (walls[:, 3].reshape([1, n]) * 0.55 + 0.05)
inside_y_mask = (inside_y_ave < 1) * (inside_y_max < 1)
inside_mask = inside_x_mask * inside_y_mask
# inside_mask[i,j] is True means walls[i] is inside of walls[j]
#print(inside_x)
#print(inside_y)
#print(inside_mask)
remain_mask = np.array([True] * n)
for i in range(n - 1):
for j in range(i + 1, n):
if inside_mask[i, j] or inside_mask[j, i]:
if inside_mask[i, j] and inside_mask[j, i]:
# (A) If inside with each other, merge two close walls
walls[j] = merge_2close_walls(walls[i], walls[j])
remain_mask[i] = False
elif inside_mask[i, j]:
# (B) If one is inside another, remove the inside one
remain_mask[i] = False
elif inside_mask[j, i]:
remain_mask[j] = False
walls_new = walls[remain_mask]
rm_n = np.sum(remain_mask)
print(f'clean close walls {n} -> {rm_n}')
if show:
show_boxes = walls0
show_boxes[:, 2] -= 0.2
show_boxes = np.concatenate([show_boxes, walls_new], 0)
Bbox3D.draw_bboxes(show_boxes, 'Z', False)
import pdb
pdb.set_trace() # XXX BREAKPOINT
#show_walls_offsetz(walls_new)
#show_walls_1by1(walls_new)
return walls_new