def draw_detections_3D(image, detections, cam, model_map): """Draws 6D detections onto resized image Parameters ---------- image: Numpy array, normalized to [0-1] detections: A list of detections for this image, coming from SSD.detect() in the form [l, t, r, b, name, confidence, 6D_pose0, ..., 6D_poseN] cam: Intrinsics for rendering model_map: Mapping of model name to Model3D instance {'obj': model3D} """ if not detections: return np.copy(image) ren = Renderer((image.shape[1], image.shape[0]), cam) ren.clear() out = np.copy(image) for det in detections: model = model_map[det[4]] for pose in det[6:]: ren.draw_model(model, pose) ren.draw_boundingbox(model, pose) col, dep = ren.finish() # Copy the rendering over into the scene mask = np.dstack((dep, dep, dep)) > 0 out[mask] = col[mask] return out
def draw_detections_3D(image, detections, cam, model_map): """Draws 6D detections onto resized image Parameters ---------- image: Numpy array, normalized to [0-1] detections: A list of detections for this image, coming from SSD.detect() in the form [l, t, r, b, name, confidence, 6D_pose0, ..., 6D_poseN] cam: Intrinsics for rendering model_map: Mapping of model name to Model3D instance {'obj': model3D} """ if not detections: return np.copy(image) ren = Renderer((image.shape[1], image.shape[0]), cam) ren.clear() out = np.copy(image) for det in detections: model = model_map[det[4]] for pose in det[6:]: ren.draw_model(model, pose) ren.draw_boundingbox(model, pose) col, dep = ren.finish() # Copy the rendering over into the scene mask = np.dstack((dep, dep, dep)) > 0 out[mask] = col[mask] return out
def precompute_projections(dataset, views, cam, model_map, models): """Precomputes the projection information needed for 6D pose construction """ save_path = f"output/{dataset}" # save the rendered images if not os.path.exists(save_path): os.makedirs(save_path) w, h = 640, 480 ren = Renderer((w, h), cam) for model_name in models: count = 0 # 图片计数 scene_camera = {} scene_gt = {} model_path = os.path.join(save_path, model_name[-6:]) rgb_dir = os.path.join(model_path, 'rgb') depth_dir = os.path.join(model_path, 'mask') ensure_dir(rgb_dir) ensure_dir(depth_dir) for i in tqdm(range(len(views))): pose = create_pose(views[i], angle_deg=0) pose[:3, 3] = [0, 0, 0.5] # zr = 0.5 model = model_map[model_name] # 随机选出一个模型 ren.clear() ren.draw_model(model, pose) # ren.draw_boundingbox(model, pose) col, dep = ren.finish() # dep 缩放因子是0.001 col *= 255 dep[dep > 0] = 255 # cam2d = project(model.fps, pose, cam) # ys, xs = np.nonzero(dep > 0) # xs_min = xs_original.min() # ys_min = ys_original.min() # xs_max = xs_original.max() # ys_max = ys_original.max() scene_camera[str(i)] = { 'cam_K': cam.flatten().tolist(), 'depth_scale': 1, 'view_level': 4, } scene_gt[str(i)] = [{ 'cam_R_m2c': pose[:3, :3].flatten().tolist(), 'cam_t_m2c': pose[:3, 3].flatten().tolist(), 'obj_id': int(model_name[-2:]) }] # draw fps # for i_p, point in enumerate(cam2d): # if i_p == 0: # col = cv2.circle(col, (point[0], point[1]), 3, (0, 255, 0), thickness=-1) # else: # col = cv2.circle(col, (point[0], point[1]), 3, (0, 0, 255), thickness=-1) cv2.imwrite(os.path.join(rgb_dir, str(count).zfill(6) + '.png'), col) cv2.imwrite(os.path.join(depth_dir, str(count).zfill(6) + '_000000' + '.png'), dep) count += 1 save_json(os.path.join(model_path, 'scene_camera.json'), scene_camera) save_json(os.path.join(model_path, 'scene_gt.json'), scene_gt)
def verify_6D_poses(detections, model_map, cam, image): """For one image, select for each detection the best pose from the 6D pool # Arguments detections: List of predictions for one image. Each prediction is: [xmin, ymin, xmax, ymax, label, confidence, (pose00), ..., (poseNM)] where poseXX is a 4x4 matrix model_map: Mapping of model name to Model3D instance {'obj': model3D} cam: Intrinsics to use for backprojection image: The scene color image # Returns filtered: List of predictions for one image. Each prediction has the form: [xmin, ymin, xmax, ymax, label, confidence, pose] where pose is a 4x4 matrix """ def compute_grads_and_mags(color): gray = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY) grads = np.dstack( (cv2.Sobel(gray, cv2.CV_32F, 1, 0, ksize=5), cv2.Sobel(gray, cv2.CV_32F, 0, 1, ksize=5))) mags = np.sqrt(np.sum(grads**2, axis=2)) + 0.001 # To avoid div/0 grads /= np.dstack((mags, mags)) mask = mags < 5 mags[mask] = 0 grads[np.dstack((mask, mask))] = 0 return grads, mags scene_grads, scene_mags = compute_grads_and_mags(image) scene_grads = np.reshape(scene_grads, (-1, 2)) #cv2.imshow('mags', scene_mags) ren = Renderer((image.shape[1], image.shape[0]), cam) filtered = [] for det in detections: model = model_map[det[4]] scores = [] for pose in det[6:]: ren.clear() ren.draw_model(model, pose) ren_grads, ren_mags = compute_grads_and_mags(ren.finish()[0]) ren_grads = np.reshape(ren_grads, (-1, 2)) dot = np.sum( np.abs(ren_grads[:, 0] * scene_grads[:, 0] + ren_grads[:, 1] * scene_grads[:, 1])) sum = np.sum(ren_mags > 0) scores.append(dot / sum) new_det = det[:6] new_det.append( det[6 + np.argmax(np.asarray(scores))]) # Put best pose first filtered.append(new_det) return filtered
def verify_6D_poses(detections, model_map, cam, image): """For one image, select for each detection the best pose from the 6D pool # Arguments detections: List of predictions for one image. Each prediction is: [xmin, ymin, xmax, ymax, label, confidence, (pose00), ..., (poseNM)] where poseXX is a 4x4 matrix model_map: Mapping of model name to Model3D instance {'obj': model3D} cam: Intrinsics to use for backprojection image: The scene color image # Returns filtered: List of predictions for one image. Each prediction has the form: [xmin, ymin, xmax, ymax, label, confidence, pose] where pose is a 4x4 matrix """ def compute_grads_and_mags(color): gray = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY) grads = np.dstack((cv2.Sobel(gray, cv2.CV_32F, 1, 0, ksize=5), cv2.Sobel(gray, cv2.CV_32F, 0, 1, ksize=5))) mags = np.sqrt(np.sum(grads**2, axis=2)) + 0.001 # To avoid div/0 grads /= np.dstack((mags, mags)) mask = mags < 5 mags[mask] = 0 grads[np.dstack((mask, mask))] = 0 return grads, mags scene_grads, scene_mags = compute_grads_and_mags(image) scene_grads = np.reshape(scene_grads, (-1, 2)) #cv2.imshow('mags', scene_mags) ren = Renderer((image.shape[1], image.shape[0]), cam) filtered = [] for det in detections: model = model_map[det[4]] scores = [] for pose in det[6:]: ren.clear() ren.draw_model(model, pose) ren_grads, ren_mags = compute_grads_and_mags(ren.finish()[0]) ren_grads = np.reshape(ren_grads, (-1, 2)) dot = np.sum(np.abs(ren_grads[:, 0]*scene_grads[:, 0] + ren_grads[:, 1]*scene_grads[:, 1])) sum = np.sum(ren_mags>0) scores.append(dot / sum) new_det = det[:6] new_det.append(det[6 + np.argmax(np.asarray(scores))]) # Put best pose first filtered.append(new_det) return filtered
def precompute_projections(views, inplanes, cam, model3D): """Precomputes the projection information needed for 6D pose construction # Arguments views: List of 3D viewpoint positions inplanes: List of inplane angles in degrees cam: Intrinsics to use for translation estimation model3D: Model3D instance # Returns data: a 3D list with precomputed entities with shape (views, inplanes, (4x4 pose matrix, 3) ) """ w, h = 640, 480 ren = Renderer((w, h), cam) data = [] if model3D.vertices is None: return data for v in tqdm(range(len(views))): data.append([]) for i in inplanes: pose = create_pose(views[v], angle_deg=i) pose[:3, 3] = [0, 0, 0.5] # zr = 0.5 # Render object and extract tight 2D bbox and projected 2D centroid ren.clear() ren.draw_model(model3D, pose) box = np.argwhere( ren.finish()[1]) # Deduct bbox from depth rendering box = [ box.min(0)[1], box.min(0)[0], box.max(0)[1] + 1, box.max(0)[0] + 1 ] centroid = np.matmul(pose[:3, :3], model3D.centroid) + pose[:3, 3] centroid_x = cam[0, 2] + centroid[0] * cam[0, 0] / centroid[2] centroid_y = cam[1, 2] + centroid[1] * cam[1, 1] / centroid[2] # Compute 2D centroid position in normalized, box-local reference frame box_w, box_h = (box[2] - box[0]), (box[3] - box[1]) norm_centroid_x = (centroid_x - box[0]) / box_w norm_centroid_y = (centroid_y - box[1]) / box_h # Compute normalized diagonal box length lr = np.sqrt((box_w / w)**2 + (box_h / h)**2) data[-1].append((pose, [norm_centroid_x, norm_centroid_y, lr])) return data
def precompute_projections(views, inplanes, cam, model3D): """Precomputes the projection information needed for 6D pose construction # Arguments views: List of 3D viewpoint positions inplanes: List of inplane angles in degrees cam: Intrinsics to use for translation estimation model3D: Model3D instance # Returns data: a 3D list with precomputed entities with shape (views, inplanes, (4x4 pose matrix, 3) ) """ w, h = 640, 480 ren = Renderer((w, h), cam) data = [] if model3D.vertices is None: return data for v in tqdm(range(len(views))): data.append([]) for i in inplanes: pose = create_pose(views[v], angle_deg=i) pose[:3, 3] = [0, 0, 0.5] # zr = 0.5 # Render object and extract tight 2D bbox and projected 2D centroid ren.clear() ren.draw_model(model3D, pose) box = np.argwhere(ren.finish()[1]) # Deduct bbox from depth rendering box = [box.min(0)[1], box.min(0)[0], box.max(0)[1] + 1, box.max(0)[0] + 1] centroid = np.matmul(pose[:3, :3], model3D.centroid) + pose[:3, 3] centroid_x = cam[0, 2] + centroid[0] * cam[0, 0] / centroid[2] centroid_y = cam[1, 2] + centroid[1] * cam[1, 1] / centroid[2] # Compute 2D centroid position in normalized, box-local reference frame box_w, box_h = (box[2] - box[0]), (box[3] - box[1]) norm_centroid_x = (centroid_x - box[0]) / box_w norm_centroid_y = (centroid_y - box[1]) / box_h # Compute normalized diagonal box length lr = np.sqrt((box_w / w) ** 2 + (box_h / h) ** 2) data[-1].append((pose, [norm_centroid_x, norm_centroid_y, lr])) return data
_, gt_pose, _ = frame.gt[0] perturbed_pose = perturb_pose(gt_pose, max_rot_pert, max_trans_pert) refinable = Refinable(model=bench.models[str(int(obj))], label=0, hypo_pose=perturbed_pose, metric_crop_shape=croppings[dataset_name]['obj_{:02d}'.format(int(obj))], input_col=col) for i in range(iterations): refinable.input_col = col.copy() start = timer() refiner.iterative_contour_alignment(refinable=refinable, max_iterations=1) end = timer() # Rendering of results ren.clear() ren.draw_background(col) ren.draw_boundingbox(refinable.model, refinable.hypo_pose) ren.draw_model(refinable.model, refinable.hypo_pose, ambient=0.5, specular=0, shininess=100, light_col=[1, 1, 1], light=[0, 0, -1]) render_col, _ = ren.finish() render_col = render_col.copy() cv2.imshow("Input Image", col) # Draw FPS in top left corner fps = "FPS: " + str(int(1 / (end - start))) cv2.rectangle(render_col, (0, 0), (133, 40), (1., 1., 1.), -1) cv2.putText(render_col, fps, (3, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2)