def show_high(windows, walls, win_high_ids, wall_high_ids): boxes = np.concatenate([windows, walls], 0) win_high_ids = np.array(win_high_ids).reshape([-1]) wall_high_ids = np.array(wall_high_ids).reshape([-1]) + windows.shape[0] high_ids = np.concatenate([win_high_ids, wall_high_ids], 0).astype(np.int) Bbox3D.draw_bboxes(boxes, 'Z', False, highlight_ids=high_ids) pass
def merge_2close_walls(bbox0, bbox1): ''' in: [n,7] [n,7] out: [n,7] centroid: mean x_size: max y_size: max z_size: max yaw: mean ''' bboxes = np.concatenate( [bbox0.reshape([-1, 1, 7]), bbox1.reshape([-1, 1, 7])], 1) bboxes_new = bboxes.mean(axis=1) bboxes_new[:, -1] = ave_angles(bboxes[:, 0, -1], bboxes[:, 1, -1], scope_id=1) bboxes_new[:, 3:6] = bboxes[:, :, 3:6].max(axis=1) show = False if show: show_boxes = np.concatenate([bboxes[0], bboxes_new], 0) show_boxes[-1, 2] += 0.2 Bbox3D.draw_bboxes(show_boxes, 'Z', False) import pdb pdb.set_trace() # XXX BREAKPOINT pass return bboxes_new
def show(self, max_num=-1, points=None, with_centroids=False, boxes_show_together=None, points_keep_rate=POINTS_KEEP_RATE, points_sample_rate=POINTS_SAMPLE_RATE, colors=None): import numpy as np from utils3d.bbox3d_ops import Bbox3D boxes = self.bbox3d.cpu().data.numpy() if with_centroids: centroids = boxes.copy() if self.mode == 'yx_zb': centroids[:,2] += centroids[:,5]*0.5 centroids[:,3:6] = 0.03 if max_num > 0 and max_num < boxes.shape[0]: step = 4 ids0 = np.random.choice(boxes.shape[0]//step-1, max_num, replace=False).reshape(-1,1)*step tmp = np.arange(step).reshape(1,step) ids = (ids0 + tmp).reshape(-1) print(ids) boxes = boxes[ids] if with_centroids: boxes = np.concatenate([boxes, centroids], 0) if boxes_show_together: boxes_show_together = boxes_show_together.bbox3d.cpu().data.numpy() labels = np.array( [0]*boxes.shape[0] + [1]*boxes_show_together.shape[0]) boxes = np.concatenate([boxes, boxes_show_together], 0) else: if 'labels' in self.fields(): labels = self.get_field('labels').cpu().data.numpy().astype(np.int32) else: labels = None if points is None: Bbox3D.draw_bboxes(boxes, 'Z', is_yx_zb=self.mode=='yx_zb', \ labels = labels, random_color=False) else: Bbox3D.draw_points_bboxes(points, boxes, 'Z', is_yx_zb=self.mode=='yx_zb',\ labels = labels, random_color=False, points_keep_rate=points_keep_rate, points_sample_rate=points_sample_rate, box_colors=colors)
def merge_pieces_of_same_walls_alongX(wall_bboxes): ''' 1) Find all the walls with not both corners intersected 2) Merge all the walls can be merged ''' if wall_bboxes.shape[0] == 0: return wall_bboxes intersections = Bbox3D.all_intersections_by_cenline( wall_bboxes, check_same_height=False, only_on_corners=True) num_inters = np.array([it.shape[0] for it in intersections]) mask = num_inters < 2 candidate_ids = np.where(mask)[0] show = DEBUG and False if show: show_boxes = wall_bboxes.copy() show_boxes[:, 2] -= 1 show_boxes = np.concatenate([show_boxes, wall_bboxes[candidate_ids]], 0) print(f'candidate_ids:{candidate_ids}') Bbox3D.draw_bboxes(show_boxes, 'Z', False) #Bbox3D.draw_bboxes(wall_bboxes[[5,6,12]], 'Z', False) pass n = candidate_ids.shape[0] keep_mask = np.array([True] * wall_bboxes.shape[0]) for i in range(n - 1): idx_i = candidate_ids[i] for j in range(i + 1, n): idx_next = candidate_ids[j] merged_i = merge_2pieces_of_1wall(wall_bboxes[idx_i], wall_bboxes[idx_next], 'X') if merged_i is not None: keep_mask[idx_i] = False wall_bboxes[idx_next] = merged_i[0] if show: show_boxes = wall_bboxes.copy() show_boxes[:, 2] -= 1 show_boxes = np.concatenate( [show_boxes, wall_bboxes[[idx_i, idx_next]], merged_i], 0) show_boxes[-1, 2] += 1 Bbox3D.draw_bboxes(show_boxes, 'Z', False) import pdb pdb.set_trace() # XXX BREAKPOINT pass wall_bboxes = wall_bboxes[keep_mask] rm_num = np.sum(1 - keep_mask) print(f'merge along X: rm {rm_num} walls') if show: show_walls_offsetz(wall_bboxes) return wall_bboxes
def show_walls_1by1(wall_bboxes): n = wall_bboxes.shape[0] for i in range(n): tmp = wall_bboxes.copy() tmp[:, 2] -= 1 show_box = np.concatenate([tmp, wall_bboxes[i:i + 1]], 0) print(f'wall {i}/{n}\n{wall_bboxes[i]}') Bbox3D.draw_bboxes(show_box, 'Z', False)
def show__together(self, boxlist_1, max_num=-1, max_num_1=-1, points=None, offset_x=None, twolabels=False, mesh=False, points_keep_rate=POINTS_KEEP_RATE, points_sample_rate=POINTS_SAMPLE_RATE, random_color=False, colors=None): import numpy as np from utils3d.bbox3d_ops import Bbox3D boxes = self.bbox3d.cpu().data.numpy().copy() if max_num > 0 and max_num < boxes.shape[0]: ids = np.random.choice(boxes.shape[0], max_num, replace=False) boxes = boxes[ids] boxes_1 = boxlist_1.bbox3d.cpu().data.numpy().copy() if max_num_1 > 0 and max_num_1 < boxes_1.shape[0]: ids = np.random.choice(boxes_1.shape[0], max_num_1, replace=False) boxes_1 = boxes_1[ids] if offset_x is not None: boxes_1[:,0] += offset_x if not twolabels and 'labels' in self.fields(): labels = self.get_field('labels').cpu().data.numpy().astype(np.int32) labels_1 = boxlist_1.get_field('labels').cpu().data.numpy().astype(np.int32) labels = np.concatenate([labels, labels_1], 0) else: labels = np.array([0]*boxes.shape[0] + [1]*boxes_1.shape[0]) boxes = np.concatenate([boxes, boxes_1], 0) if colors is not None: colors = np.concatenate(colors, 0) assert colors.shape[0] == boxes.shape[0] if points is None: if mesh: Bbox3D.draw_bboxes_mesh(boxes, 'Z', is_yx_zb=self.mode=='yx_zb', labels=labels) else: Bbox3D.draw_bboxes(boxes, 'Z', is_yx_zb=self.mode=='yx_zb', labels=labels, random_color=False) else: if isinstance(points, torch.Tensor): points = points.cpu().data.numpy() if offset_x is not None: tp = points.copy() tp[:,0] += offset_x points = np.concatenate([points, tp], 0) # tp = tp.copy() tp[:,0] += offset_x points = np.concatenate([points, tp], 0) if mesh: Bbox3D.draw_points_bboxes_mesh(points, boxes, 'Z', is_yx_zb=self.mode=='yx_zb', labels=labels, points_keep_rate=points_keep_rate, points_sample_rate=points_sample_rate, random_color=random_color, box_colors=colors) else: Bbox3D.draw_points_bboxes(points, boxes, 'Z', is_yx_zb=self.mode=='yx_zb', labels=labels, random_color=random_color, points_keep_rate=points_keep_rate, points_sample_rate=points_sample_rate, box_colors=colors)
def show_highlight(self, ids, points=None): from utils3d.bbox3d_ops import Bbox3D ids = np.array(ids) n = len(self) labels = np.zeros([n]).astype(np.int) labels[ids] = 1 boxes = self.bbox3d.cpu().data.numpy() if points is None: Bbox3D.draw_bboxes(boxes, 'Z', is_yx_zb=self.mode=='yx_zb', labels=labels, random_color=False) else: if isinstance(points, torch.Tensor): points = points.cpu().data.numpy() Bbox3D.draw_points_bboxes(points, boxes, 'Z', is_yx_zb=self.mode=='yx_zb', labels=labels, random_color=False)
import numpy as np from wall_preprocessing import merge_2pieces_of_1wall from utils3d.bbox3d_ops import Bbox3D box0 = np.array([[ 43.59999866, -41.88736687, 1.36749997, 3.17999993, 0.09473495, 2.73499994, 0. ]]) box1 = np.array([[ 43.79499936, -41.95263138, 1.36749997, 2.80999994, 0.09473877, 2.73499994, 0. ]]) box_merge = merge_2pieces_of_1wall(box0, box1, 'Y') print(box_merge) show_boxes = np.concatenate([box0, box1], 0) Bbox3D.draw_bboxes(show_boxes, 'Z', False)
def get_part_bbox(vertices, triangle, triangle_norms, name=''): ''' bbox: [xc, yc, zc, x_size, y_size, z_size, yaw] ''' #show_mesh(vertices, triangle) class_name = name.split('#')[0] box_min = np.min(vertices, 0) box_max = np.max(vertices, 0) centroid = (box_min + box_max) / 2.0 y_size = box_max[1] - box_min[1] ## Find the 8 outside corners distances = np.linalg.norm(vertices - np.expand_dims(centroid, 0), axis=1) max_dis = max(distances) out_corner_mask = (abs(distances - max_dis) < 1e-5) n0 = vertices.shape[0] #print([i for i in range(n0) if out_corner_mask[i]]) out_vertices = [ vertices[i:i + 1, :] for i in range(n0) if out_corner_mask[i] ] if len(out_vertices) == 0: import pdb pdb.set_trace() # XXX BREAKPOINT pass return None out_vertices = np.concatenate(out_vertices, 0) if out_vertices.shape[0] != 8: #Bbox3D.draw_points_open3d(out_vertices, show=True) #Bbox3D.draw_points_open3d(vertices, show=True) #show_mesh(vertices, triangle) if class_name not in ENABLE_NO_RECTANGLE: print( f'\nFailed to find bbox, not rectangle, {class_name} \n {out_vertices.shape[0]} vertices\n' ) assert False import pdb pdb.set_trace() # XXX BREAKPOINT pass return None else: print( f'\nNot rectangle, use no yaw box, {class_name} \n {out_vertices.shape[0]} vertices\n' ) min_max = {'min': box_min, 'max': box_max} bbox_noyaw = Bbox3D.bbox_from_minmax(min_max) #Bbox3D.draw_points_bboxes(vertices, bbox_noyaw, 'Y', is_yx_zb=False) return bbox_noyaw ## Find the 4 corners on one side x_right_mask = out_vertices[:, 0] - centroid[0] > 0 x_right_corners = np.concatenate( [out_vertices[i:i + 1, :] for i in range(8) if x_right_mask[i]], 0) assert x_right_corners.shape[0] == 4 x_right_cen = np.mean(x_right_corners, 0) ## get length x_size = np.linalg.norm(x_right_cen - centroid) * 2 if not abs(x_right_cen[1] - centroid[1]) < 1e-7: print("y should be same, pitch and rool should be 0") ## the angle between x_right_cen and x axis x_direc = x_right_cen - centroid x_direc = (x_direc) / np.linalg.norm(x_direc) x_axis = np.array([1, 0, 0]) # [-90, 90] yaw = np.arccos(np.sum(x_direc * x_axis)) if abs(yaw) < 0.01: yaw = 0 else: assert x_direc[0] > 0 assert x_direc[1] == 0 yaw *= np.sign(x_direc[2]) ## Find the 2 corners at top within x_right_corners top_mask = x_right_corners[:, 1] - centroid[1] > 0 x_right_top_corners = np.concatenate( [x_right_corners[i:i + 1, :] for i in range(4) if top_mask[i]], 0) assert x_right_top_corners.shape[0] == 2 # get z_size z_size = np.linalg.norm(x_right_top_corners[0] - x_right_top_corners[1]) ### got the bbox xc, yc, zc = centroid bbox = np.array([xc, yc, zc, x_size, y_size, z_size, yaw]) ### Check if yaw != 0 and False: Bbox3D.draw_bboxes(bbox, 'Y', False) if False: min_max = {'min': box_min, 'max': box_max} bbox_noyaw = Bbox3D.bbox_from_minmax(min_max) bboxes_show = np.concatenate( [bbox.reshape([1, 7]), bbox_noyaw.reshape([1, 7])], 0) Bbox3D.draw_points_bboxes(vertices, bboxes_show, 'Y', is_yx_zb=False) return bbox
def show_walls_offsetz(wall_bboxes): n = wall_bboxes.shape[0] wall_bboxes = wall_bboxes.copy() wall_bboxes[:, 2] += np.random.rand(n) * 1 print(f'totally {n} boxes') Bbox3D.draw_bboxes(wall_bboxes, 'Z', False)
def merge_2pieces_of_1wall(bbox0, bbox1, dim): ''' [1,7] [1,7] two box can merge, when: z, size_y, size_z same yaw0 = yaw1 = angle of c0_c1 out: [1,7] ''' from utils3d.geometric_util import angle_with_x assert dim == 'X' or 'Y' dim = 0 if dim == 'X' else 1 bbox0 = bbox0.reshape([1, 7]) bbox1 = bbox1.reshape([1, 7]) dif = bbox1 - bbox0 dif[0, -1] = limit_period(dif[0, -1], 0.5, np.pi) z_same = np.abs(dif[0, 2]) < 0.01 if dim == 0: # o means the other here, if dim=='X', o='Y' so_same = np.abs(dif[0, 3 + 1 - dim]) < 0.05 else: so_same = np.abs(dif[0, 3 + 1 - dim]) < 0.15 sz_same = np.abs(dif[0, 3 + 2]) < 0.01 z_sames0 = z_same and sz_same if z_sames0: z_sames = z_sames0 else: z0_max = bbox0[0, 2] + bbox0[0, 5] * 0.5 z0_min = bbox0[0, 2] - bbox0[0, 5] * 0.5 z1_max = bbox1[0, 2] + bbox1[0, 5] * 0.5 z1_min = bbox1[0, 2] - bbox1[0, 5] * 0.5 zmin_dif = np.abs(z1_min - z0_min) zmax_dif = np.abs(z1_max - z0_max) zmin_same = zmin_dif < 0.01 zmax_same = zmax_dif < 0.03 z_sames = zmin_same and zmax_same if not zmax_same: iou_xy = rotate_iou_gpu(bbox0[:, [0, 1, 3, 4, 6]], bbox1[:, [0, 1, 3, 4, 6]]) #print(f'box0 : {bbox0}') #print(f'box1 : {bbox1}') print( f'zmin_dif:{zmin_dif}, zmax_dif:{zmax_dif}\n iou_xy:{iou_xy}\n' ) if iou_xy > MERGE_Z_ANYWAY_XYIOU_THRESHOLD: print('Merge even z is different') z_sames = True else: print('abort merging because of z is different') if DEBUG and False: box_show = np.concatenate([bbox0, bbox1], 0) Bbox3D.draw_bboxes(box_show, 'Z', False) import pdb pdb.set_trace() # XXX BREAKPOINT pass if z_sames: zmin_new = min(z0_min, z1_min) zmax_new = max(z0_max, z1_max) z_new = (zmin_new + zmax_new) / 2.0 z_size_new = zmax_new - zmin_new bbox0[0, 2] = z_new bbox0[0, 5] = z_size_new bbox1[0, 2] = z_new bbox1[0, 5] = z_size_new yaw_same = np.abs(dif[0, -1]) < 0.05 if not (z_sames and so_same and yaw_same): return None cen_direc = dif[:, 0:2] if dim == 0: cen_line0 = Bbox3D.bboxes_centroid_lines(bbox0, 'X', 'Z') dis_01 = vertical_dis_1point_lines(bbox1[0, 0:3], cen_line0)[0] overlap_mask0 = dis_01 < bbox0[0, 4] * 0.51 + 0.01 if not overlap_mask0: return None cen_dis = np.linalg.norm(dif[0, 0:3]) overlap_mask1 = cen_dis < (bbox0[0, 3] + bbox1[0, 3]) * 0.5 + 0.01 if not overlap_mask1: return None centroid_lines1 = Bbox3D.bboxes_centroid_lines(bbox1, 'X' if dim == 1 else 'Y', 'Z') cen_dis = vertical_dis_1point_lines(bbox0[0, 0:3], centroid_lines1)[0] size_dim0 = bbox0[0, 3 + dim] size_dim1 = bbox1[0, 3 + dim] is_box0_covered_by_1 = size_dim1 * 0.5 > cen_dis + size_dim0 * 0.5 is_box1_covered_by_0 = size_dim0 * 0.5 > cen_dis + size_dim1 * 0.5 if is_box0_covered_by_1: merged = bbox1 elif is_box1_covered_by_0: merged = bbox0 else: k = size_dim1 / (size_dim0 + size_dim1) new_centroid = bbox0[0, 0:3] + (bbox1[0, 0:3] - bbox0[0, 0:3]) * k new_size_dim = (size_dim0 + size_dim1) / 2 + cen_dis merged = (bbox0 + bbox1) / 2 merged[:, -1] = ave_angles(bbox0[:, -1], bbox1[:, -1], scope_id=1) if np.abs(bbox1[0, -1] - bbox0[0, -1]) > np.pi * 0.5: merged[0, -1] = (np.abs(bbox0[0, -1]) + np.abs(bbox1[0, -1])) / 2.0 merged[0, 0:3] = new_centroid merged[0, 3 + dim] = new_size_dim # show = False if show: box_show = np.concatenate([bbox0, bbox1, merged], 0) box_show[2, 2] += 0.1 Bbox3D.draw_bboxes(box_show, 'Z', False) import pdb pdb.set_trace() # XXX BREAKPOINT pass return merged
def clean_close_walls(wall_bboxes): from second.core.non_max_suppression.nms_gpu import rotate_iou_gpu, rotate_iou_gpu_eval #Bbox3D.draw_bboxes(wall_bboxes, 'Z', False) walls_2d = wall_bboxes[:, [0, 1, 3, 4, 6]] show_merge = False wall_bboxes_0 = wall_bboxes.copy() wall_bboxes_0[:, 2] -= 1 n = wall_bboxes.shape[0] keep_mask = np.array([True] * n) for i in range(n): for j in range(i + 1, n): #iou_ij = ious[i,j] #if iou_ij <= 0.05: # continue #if ious[i,j] >= 0.8: # close_ij = True #else: close_ij = is_close_2walls(wall_bboxes[i], wall_bboxes[j]) if not close_ij: continue if close_ij: keep_mask[i] = False merged_i = merge_2close_walls(wall_bboxes[i], wall_bboxes[j]) show_merge = False else: merged_i = np.zeros(shape=[0, 7], dtype=np.float32) show_merge = False if show_merge: #print(f'iou: {ious[i,j]}\nfA:{wall_bboxes[i]}\nB:{wall_bboxes[j]}\nM:{merged_i}') merged_show = merged_i.copy() merged_show[:, 2] += 1 box_show = np.concatenate( [ wall_bboxes[i:i + 1], wall_bboxes[j:j + 1], #merged_show],0) merged_show, wall_bboxes_0 ], 0) box_show[0, 2] += 0.2 Bbox3D.draw_bboxes(box_show, 'Z', False) import pdb pdb.set_trace() # XXX BREAKPOINT pass if not close_ij: print( "2 walls are a bit close, but not close enough to merge" + \ "This should not happen.") #raise NotImplementedError else: wall_bboxes[j] = merged_i wall_bboxes_new = wall_bboxes[keep_mask] merge_num = n - wall_bboxes_new.shape[0] print(f'wall num: {n} -> {wall_bboxes_new.shape[0]}') #walls_2d = wall_bboxes_new[:,[0,1,3,4,6]] #ious = rotate_iou_gpu(walls_2d, walls_2d) #show_walls_offsetz(wall_bboxes_new) #show_walls_1by1(wall_bboxes_new) return wall_bboxes_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
def merge_pieces_of_same_walls_alongY(wall_bboxes): ''' 1) Find all the walls which are A) parallel along X, B) one of cen_corners is very close C) centroid of one wall is inside of another 2) Merge along Y ''' from utils3d.geometric_util import angle_with_x, vertical_dis_points_lines if wall_bboxes.shape[0] == 0: return wall_bboxes show = False if show: wall_bboxes0 = wall_bboxes.copy() wall_bboxes0[:, 2] -= 1 n = wall_bboxes.shape[0] cen_lines = Bbox3D.bboxes_centroid_lines(wall_bboxes, cen_axis='X', up_axis='Z') # [n,2,3] cen_direc = cen_lines[:, 1, :] - cen_lines[:, 0, :] # [n,3] np.linalg.norm(cen_direc[:, 0:2], axis=1) angles = angle_with_x(cen_direc[:, 0:2], 1) boxes_merged_extra = [] remain_mask = np.array([True] * n) merge_y_num = 0 split_and_merge_num = 0 for i in range(n - 1): # 1) parallel along X angles_dif = angles[i] - angles[i + 1:] # make to [0,np.pi/2] angles_dif = np.abs(limit_period(angles_dif, 0.5, np.pi)) angle_mask = angles_dif < 7 * np.pi / 180 # 2) one of centroid line corners is very close cen_dis = cen_lines[i:i + 1].reshape( [1, 2, 1, 3]) - cen_lines[i + 1:].reshape([-1, 1, 2, 3]) cen_dis = np.linalg.norm(cen_dis, axis=3) cen_dis_min = cen_dis.min(axis=1).min(axis=1) cen_dis_max = cen_dis.max(axis=1).max(axis=1) thickness_sum = wall_bboxes[:, 4] * 0.5 + wall_bboxes[i, 4] * 0.5 thickness_sum = thickness_sum[i + 1:] cendis_mask = cen_dis_min < thickness_sum # 3) the other centroid line corner is inside of the long box centroid_dis = np.linalg.norm(wall_bboxes[i:i + 1, 0:3] - wall_bboxes[i + 1:, 0:3], axis=1) #tmp = np.maximum(wall_bboxes[i,3], wall_bboxes[i+1:,3]) tmp = (wall_bboxes[i, 3] + wall_bboxes[i + 1:, 3]) * 0.45 - 0.1 centroid_mask = centroid_dis < tmp #if i==4: # import pdb; pdb.set_trace() # XXX BREAKPOINT # show_boxes = wall_bboxes[[4,12]] # Bbox3D.draw_bboxes(show_boxes, 'Z', False) # pass mask_i = angle_mask * cendis_mask * centroid_mask if mask_i.any(): ids = np.where(mask_i)[0] + i + 1 # 5) the centroid corner used to split the other # 4) vertical dis vertical_dis = vertical_dis_points_lines(cen_lines[i], cen_lines[ids]).mean(0) ave_thickness = (wall_bboxes[i, 4] + wall_bboxes[ids, 4]) * 0.5 thickness_gap_rate = vertical_dis / ave_thickness verdis_mask = (thickness_gap_rate > 0.2) * (thickness_gap_rate < 1.2) ids = ids[verdis_mask] vertical_dis = vertical_dis[verdis_mask] is_need_merge = ids.shape[0] > 0 if is_need_merge: # split the longer box, and merge the shorter one along Y k = ids.shape[0] for j in range(k): idx = ids[j] size_x_i = wall_bboxes[i, 3] size_x_j = wall_bboxes[idx, 3] size_x_rate = size_x_i / size_x_j print(f'size_x_rate: {size_x_rate}') if np.abs(size_x_rate - 1) < 0.15: merge_y_num += 1 box_merge = merge_2pieces_of_1wall( wall_bboxes[i], wall_bboxes[idx], 'Y') if show and False: show_boxes = np.concatenate([ wall_bboxes[i:i + 1], wall_bboxes[idx:idx + 1] ], 0) if box_merge is not None: show_boxes = np.concatenate( [show_boxes, box_merge], 0) show_boxes[-1, 2] += 0.2 else: import pdb pdb.set_trace() # XXX BREAKPOINT pass show_boxes = np.concatenate( [show_boxes, wall_bboxes0], 0) Bbox3D.draw_bboxes(show_boxes, 'Z', False) pass if box_merge is not None: wall_bboxes[idx] = box_merge.reshape([7]) remain_mask[i] = False else: print( 'The two walls cannot be merged along Y, this should not happen normally' ) print( 'merge_pieces_of_same_walls_alongY again after crop_walls to solve some special case: Originally, a wall is broken by no intersection like 0058113bdc8bee5f387bb5ad316d7b28' ) #show_boxes = np.concatenate([wall_bboxes[i:i+1], wall_bboxes[idx:idx+1]], 0) #Bbox3D.draw_bboxes(wall_bboxes, 'Z', False, highlight_ids=[idx, i]) #Bbox3D.draw_bboxes(show_boxes, 'Z', False) #import pdb; pdb.set_trace() # XXX BREAKPOINT #pass #raise NotImplementedError else: # the longer box need to be split along X before merging split_and_merge_num += 1 cen_dis_ij = cen_dis[idx - i - 1] if size_x_rate > 1: longer_idx = i short_idx = idx cen_dis_ij = cen_dis_ij.min(axis=0) else: longer_idx = idx short_idx = i cen_dis_ij = cen_dis_ij.min(axis=1) # find the intersection point on longer box far_corner_id = int(cen_dis_ij[0] < cen_dis_ij[1]) cen_dir_longer = cen_lines[longer_idx, 1] - cen_lines[longer_idx, 0] cen_dir_longer /= np.linalg.norm(cen_dir_longer) sl_dir = cen_lines[ short_idx, far_corner_id] - cen_lines[longer_idx, 0] intersection = np.sum( sl_dir * cen_dir_longer ) * cen_dir_longer + cen_lines[longer_idx, 0] # offset: half of the thickness intersection += cen_dir_longer * wall_bboxes[i, 4] * 0.5 splited_boxes = Bbox3D.split_wall_by_centroid_intersections( wall_bboxes[longer_idx], intersection.reshape([1, 3])) # [2,7] if splited_boxes.shape[0] == 1: print( '\n\n\t\tComplicated situation, not solved well yet.\n\n' ) box_merge = None if False and DEBUG and splited_boxes.shape[0] == 1: box_tmp = np.array( [[0, 0, 0, 0.5, 0.5, 0.5, 0]]) box_tmp[0, 0:3] = intersection.reshape([1, 3]) boxes_show = np.concatenate([ box_tmp, wall_bboxes[longer_idx].reshape( [-1, 7]) ], 0) Bbox3D.draw_points_bboxes( intersection.reshape([1, 3]), boxes_show, 'Z', False) else: tmp = wall_bboxes[short_idx, 0:3] - splited_boxes[:, 0: 3] # [2,3] tmp = np.linalg.norm(tmp, axis=1) # [2] merge_id = int(tmp[0] > tmp[1]) box_merge = merge_2pieces_of_1wall( wall_bboxes[short_idx], splited_boxes[merge_id], 'Y') if show and False: #if box_merge is None: show_boxes = np.concatenate([ wall_bboxes[i:i + 1], wall_bboxes[idx:idx + 1] ], 0) Bbox3D.draw_points_bboxes( intersection.reshape([1, 3]), show_boxes, 'Z', False) show_boxes = np.concatenate( [show_boxes, splited_boxes], 0) show_boxes[-1, 2] += 0.5 show_boxes[-2, 2] += 0.7 if box_merge is not None: show_boxes = np.concatenate( [show_boxes, box_merge], 0) show_boxes[-1, 2] += 1 show_boxes = np.concatenate( [show_boxes, wall_bboxes0], 0) Bbox3D.draw_bboxes(show_boxes, 'Z', False) import pdb pdb.set_trace() # XXX BREAKPOINT pass if box_merge is None: # temperally solution, may because of a wall being changed several # times remain_mask[short_idx] = False else: wall_bboxes[longer_idx] = splited_boxes[1 - merge_id] wall_bboxes[short_idx] = box_merge.reshape([7]) boxes_merged_extra.append(box_merge.reshape([1, 7])) wall_bboxes_new = wall_bboxes[remain_mask] #boxes_merged_extra = np.concatenate(boxes_merged_extra, 0) #wall_bboxes_new = np.concatenate([wall_bboxes, boxes_merged_extra], 0) print( f'merge_y_num:{merge_y_num} split_and_merge_num:{split_and_merge_num}' ) if show: show_walls_offsetz(wall_bboxes_new) #show_walls_offsetz(wall_bboxes) #show_walls_offsetz(wall_bboxes0[remain_mask]) show_walls_offsetz(wall_bboxes0[np.logical_not(remain_mask)]) #show_walls_offsetz(boxes_merged_extra) return wall_bboxes_new