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