def optimizeSuperpixels(self, x): x = x.reshape(6, ).copy() base_frame = self.instance_frame # x = np.multiply(x, self.gains) frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT) frame = base_frame * frame matrix = KLDtoNumpyMatrix(frame) R = matrix[:3, :3] t = matrix[:3, 3] * 1000.0 ren_rgb = renderer.render(self.model, self.im_size, self.K, R, t, mode='rgb', surf_color=[0, 1.0, 0]) nonzeroindices = np.argwhere(ren_rgb > 0) labels = set(ariadne.graph.labels[nonzeroindices[:, 0], nonzeroindices[:, 1]]) # print("LABELS", ) # for index in nonzeroindices: # label = ariadne.graph.labels[index[0], index[1]] # labels[label] = True zero = np.zeros(self.image.shape[:2]) output_image = self.output_image.copy() for l in labels: label_indices = np.argwhere(ariadne.graph.labels == l) output_image[label_indices[:, 0], label_indices[:, 1]] = np.array([255, 255, 255]).astype(np.uint8) zero[label_indices[:, 0], label_indices[:, 1]] = np.array([255]).astype(np.uint8) output_image[nonzeroindices[:, 0], nonzeroindices[:, 1]] = np.array([255, 0, 255]).astype(np.uint8) zero[nonzeroindices[:, 0], nonzeroindices[:, 1]] = 0 # cv2.imshow("model", ren_rgb) # cv2.imshow("image", output_image) # cv2.imshow("zero", zero) # c = cv2.waitKey(1) count = np.count_nonzero(zero.ravel()) if count == 0: count = np.inf # print("CURENT X", x, count) return count
def optimizeRGBDifference(self, x): x = x.reshape(6,).copy() base_frame = self.instance_frame x = np.multiply(x, self.gains) if np.linalg.norm(x[:3]) > 0.2: return np.inf # x[3:] = x[3:]*180/math.pi frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT) frame = base_frame*frame matrix = KLDtoNumpyMatrix(frame) R = matrix[:3, :3] t = matrix[:3, 3]*1000.0 ren_rgb = renderer.render( self.model, self.img_size, self.K, R, t, mode='rgb') # , surf_color=(76.0/255.0, 72.0/255.0, 82.0/255.0)) gray = self.gray rgb_lines = lsd.detect(gray)[0] model_lines = lsd.detect(cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY))[0] zeros = np.zeros(self.gray.shape) rgb_lines_img = self.drawSegments(zeros, rgb_lines) model_lines_img = self.drawSegments(zeros, model_lines) ren_rgb = cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY) rendered_image = gray.copy() rendered_image[ren_rgb != 0] = ren_rgb[ren_rgb != 0] grayf = gray.astype(float) rendered_imagef = rendered_image.astype(float) diff = np.abs(grayf - rendered_imagef)/255.0 # rgb_lines_img = lsd.drawSegments(zeros, rgb_lines) # model_lines_img = lsd.drawSegments(zeros, model_lines) count = np.sum(diff) print("MAXMIN", np.min(diff), np.max(diff)) cv2.imshow("model", rendered_image) cv2.imshow("diff", diff) cv2.waitKey(10) return count
def optimize(self, x): x = x.reshape(6, ).copy() base_frame = self.instance_frame x = np.multiply(x, self.gains) frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT) frame = base_frame * frame matrix = KLDtoNumpyMatrix(frame) R = matrix[:3, :3] t = matrix[:3, 3] * 1000.0 ren_rgb = renderer.render(self.model, self.img_size, self.K, R, t, mode='rgb', surf_color=(1, 1, 1), ambient_weight=0.0) rgb_lines = lsd.detect(self.gray)[0] model_lines = lsd.detect(cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY))[0] zeros = np.zeros(self.gray.shape) rgb_lines_img = self.drawSegments(zeros, rgb_lines) model_lines_img = self.drawSegments(zeros, model_lines) rendered_image = cv2.addWeighted(self.image, 1, ren_rgb, 0.85, 0) # rgb_lines_img = lsd.drawSegments(zeros, rgb_lines) # model_lines_img = lsd.drawSegments(zeros, model_lines) # diff = rgb_lines_img - model_lines_img diff = cv2.bitwise_and(rgb_lines_img, model_lines_img) # diff = np.abs(diff.astype(np.uint8)) count = np.count_nonzero(diff.ravel()) print("MAXMIN", np.min(diff), np.max(diff), count) cv2.imshow("model", rendered_image) cv2.imshow("opt_rgb", rgb_lines_img) cv2.imshow("opt_model", model_lines_img) cv2.imshow("opt_diff", diff) cv2.waitKey(10) return -count
def render(self, rgb, model, R, t): K = self.get_camera_intrinsic() surf_color = (1, 0, 0) im_size = (rgb.shape[1], rgb.shape[0]) ren_rgb = renderer.render(model, im_size, K, R, t, surf_color=surf_color, mode='rgb') ren_gray = cv2.cvtColor(ren_rgb, cv2.COLOR_RGB2GRAY) mask = np.zeros((ren_gray.shape[0], ren_gray.shape[1])) mask[ren_gray != 0] = 255 # cv2.imwrite("/home/david/test.png",mask) vis_rgb = 0.4 * rgb.astype(np.float) + 0.6 * ren_rgb.astype(np.float) vis_rgb = vis_rgb.astype(np.uint8) return vis_rgb, mask
def optimizeGradientOrientation(self, x): x = x.reshape(7, ).copy() base_frame = self.instance_frame frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT) frame = base_frame * frame matrix = KLDtoNumpyMatrix(frame) R = matrix[:3, :3] t = matrix[:3, 3] * 1000.0 ren_rgb = renderer.render(self.model, self.im_size, self.K, R, t, mode='rgb', surf_color=[100, 100, 100]) image_angle = self.getGradientOrientation(self.image) model_angle = self.getGradientOrientation(ren_rgb) mask = np.zeros(image_angle.shape) mask[model_angle > 0] = image_angle[model_angle > 0] diff = np.abs(mask - model_angle) e = np.sum(diff.ravel()) output = image.copy() output[ren_rgb > 0] = ren_rgb[ren_rgb > 0] cv2.imshow("image", image_angle) cv2.imshow("model", model_angle) cv2.imshow("mask", diff) cv2.imshow("output", output) cv2.waitKey(1) print("Error", e) return e
instance_frame = instances_poses[0]*transform instance_frame = poses[index].Inverse() * instance_frame if optimized_correction is not None: print("CORRECTING WIGH", optimized_correction) instance_frame = instance_frame*optimized_correction instance_frame_matrix = KLDtoNumpyMatrix(instance_frame) R = instance_frame_matrix[:3, :3] t = instance_frame_matrix[:3, 3]*1000.0 im_size = (640, 480) ren_rgb = renderer.render(model, im_size, K, R, t, mode='rgb', surf_color=[0, 1.0, 0]) # rgb_lines = lsd.detect(gray)[0] # model_lines = lsd.detect(cv2.cvtColor(ren_rgb, cv2.COLOR_BGR2GRAY))[0] # zeros = np.zeros(gray.shape) # rgb_lines_img = lsd.drawSegments(zeros, rgb_lines) # model_lines_img = lsd.drawSegments(zeros, model_lines) if optimized_correction is None: lopt = LineOptimizer(K, image, model, instance_frame) eye = PyKDL.Frame() cv2.imshow("model", image) cv2.waitKey(0)
# Visualization #1 #----------------------------------------------------------------------- # Load RGB image rgb_path = rgb_path_mask.format(device, obj_id, im_id, rgb_ext[device]) rgb = imageio.imread(rgb_path) # Render RGB image of the object model at the pose associated with # the training image into a # surf_color = obj_colors[obj_id] surf_color = (0, 1, 0) im_size = (rgb.shape[1], rgb.shape[0]) ren_rgb = renderer.render(model, im_size, K, R, t, surf_color=surf_color, mode='rgb') vis_rgb = 0.5 * rgb.astype(np.float) + 0.5 * ren_rgb.astype(np.float) vis_rgb = vis_rgb.astype(np.uint8) # Draw the bounding box of the object vis_rgb = misc.draw_rect(vis_rgb, im_gt[0]['obj_bb']) # Save the visualization vis_rgb[vis_rgb > 255] = 255 vis_rgb_path = vis_rgb_path_mask.format(obj_id, device, model_type, im_id) imageio.imwrite(vis_rgb_path, vis_rgb.astype(np.uint8))
rgb_path = rgb_path_mask.format(device, scene_id, im_id, rgb_ext[device]) rgb = imageio.imread(rgb_path) im_size = (rgb.shape[1], rgb.shape[0]) vis_rgb = np.zeros(rgb.shape, np.float) for gt in scene_gt[im_id]: model = models[gt['obj_id']] R = gt['cam_R_m2c'] t = gt['cam_t_m2c'] surf_color = obj_colors[gt['obj_id'] - 1] ren_rgb = renderer.render(model, im_size, K, R, t, surf_color=surf_color, mode='rgb') # Draw the bounding box of the object ren_rgb = misc.draw_rect(ren_rgb, gt['obj_bb']) vis_rgb += 0.7 * ren_rgb.astype(np.float) # cv2.imshow('step vis', ren_rgb) # cv2.waitKey() # Save the visualization vis_rgb = 0.6 * vis_rgb + 0.4 * rgb vis_rgb[vis_rgb > 255] = 255 vis_rgb_path = vis_rgb_path_mask.format(scene_id, device, model_type,
def test(ctx): config = ctx.obj['config'] scene_ids = range(1, 21) device = config['device'] model_type = config['model_type'] im_step = int(config['image_step']) data_path = config['dataset_path'] output_dir = config['output_path'] # Paths to the elements of the T-LESS dataset model_path_mask = os.path.join(data_path, 'models_' + model_type, 'obj_{:02d}.ply') scene_info_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'info.yml') scene_gt_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'gt.yml') rgb_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'rgb', '{:04d}.{}') depth_path_mask = os.path.join(data_path, 'test_{}', '{:02d}', 'depth', '{:04d}.png') rgb_ext = {'primesense': 'png', 'kinect': 'png', 'canon': 'jpg'} obj_colors_path = os.path.join('data', 'obj_rgb.txt') vis_rgb_path_mask = os.path.join(output_dir, '{:02d}_{}_{}_{:04d}_rgb.png') vis_depth_path_mask = os.path.join(output_dir, '{:02d}_{}_{}_{:04d}_depth_diff.png') misc.ensure_dir(output_dir) obj_colors = inout.load_colors(obj_colors_path) plt.ioff() # Turn interactive plotting off for scene_id in scene_ids: # Load info about the test images (including camera parameters etc.) scene_info_path = scene_info_path_mask.format(device, scene_id) scene_info = inout.load_scene_info(scene_info_path) scene_gt_path = scene_gt_path_mask.format(device, scene_id) gts = inout.load_scene_gt(scene_gt_path) # Load models of objects present in the scene scene_obj_ids = set() for gt in gts[0]: scene_obj_ids.add(gt['obj_id']) models = {} for scene_obj_id in scene_obj_ids: model_path = model_path_mask.format(scene_obj_id) models[scene_obj_id] = inout.load_ply(model_path) for im_id, im_info in scene_info.items(): if im_id % im_step != 0: continue print('scene: ' + str(scene_id) + ', device: ' + device + ', im_id: ' + str(im_id)) # Get intrinsic camera parameters K = im_info['cam_K'] # Visualization #1 # ----------------------------------------------------------------------- # Load RGB image rgb_path = rgb_path_mask.format(device, scene_id, im_id, rgb_ext[device]) rgb = scipy.misc.imread(rgb_path) im_size = (rgb.shape[1], rgb.shape[0]) vis_rgb = np.zeros(rgb.shape, np.float) for gt in gts[im_id]: model = models[gt['obj_id']] R = gt['cam_R_m2c'] t = gt['cam_t_m2c'] surf_color = obj_colors[gt['obj_id'] - 1] ren_rgb = renderer.render(model, im_size, K, R, t, surf_color=surf_color, mode='rgb') import cv2 cv2.imshow("test", ren_rgb) cv2.waitKey() # Draw the bounding box of the object ren_rgb = misc.draw_rect(ren_rgb, gt['obj_bb']) vis_rgb += 0.7 * ren_rgb.astype(np.float) # Save the visualization vis_rgb = 0.6 * vis_rgb + 0.4 * rgb vis_rgb[vis_rgb > 255] = 255 vis_rgb_path = vis_rgb_path_mask.format(scene_id, device, model_type, im_id) scipy.misc.imsave(vis_rgb_path, vis_rgb.astype(np.uint8)) # Visualization #2 # ----------------------------------------------------------------------- if device != 'canon': # Load depth image depth_path = depth_path_mask.format(device, scene_id, im_id, rgb_ext[device]) depth = scipy.misc.imread(depth_path) # Unit: 0.1 mm depth = depth.astype(np.float) * 0.1 # Convert to mm # Render the objects at the ground truth poses im_size = (depth.shape[1], depth.shape[0]) ren_depth = np.zeros(depth.shape, np.float) for gt in gts[im_id]: model = models[gt['obj_id']] R = gt['cam_R_m2c'] t = gt['cam_t_m2c'] # Render the current object ren_depth_obj = renderer.render(model, im_size, K, R, t, mode='depth') # Add to the final depth map only the parts of the surface that # are closer than the surfaces rendered before visible_mask = np.logical_or(ren_depth == 0, ren_depth_obj < ren_depth) mask = np.logical_and(ren_depth_obj != 0, visible_mask) ren_depth[mask] = ren_depth_obj[mask].astype(np.float) # Calculate the depth difference at pixels where both depth maps # are valid valid_mask = (depth > 0) * (ren_depth > 0) depth_diff = valid_mask * (depth - ren_depth.astype(np.float)) # Save the visualization vis_depth_path = vis_depth_path_mask.format( scene_id, device, model_type, im_id) plt.matshow(depth_diff) plt.title('captured - rendered depth [mm]') plt.colorbar() plt.savefig(vis_depth_path, pad=0) plt.close()
def optimizeSuperpixelsReduced(self, x): x = x.reshape(7, ).copy() base_frame = self.instance_frame #x = np.multiply(x, self.gains) frame = KDLFromArray(x, fmt=LineOptimizer.DEFAULT_FORMAT) frame = base_frame * frame matrix = KLDtoNumpyMatrix(frame) R = matrix[:3, :3] t = matrix[:3, 3] * 1000.0 ren_rgb = renderer.render(self.model, self.im_size, self.K, R, t, mode='rgb', surf_color=[0, 1.0, 0]) nonzeroindices = np.argwhere(ren_rgb > 0) labels_raw = ariadne.graph.labels[nonzeroindices[:, 0], nonzeroindices[:, 1]].ravel() labels = np.unique(labels_raw) y = np.bincount(labels_raw) ii = np.nonzero(y)[0] labels_count = dict(zip(ii, y[ii])) labels_variance = np.var(y[ii]) #print("LABELS_COUNT", labels_count, dict(labels_count)) # And then: # print("LABELS", labels) # for index in nonzeroindices: # label = ariadne.graph.labels[index[0], index[1]] # labels[label] = True zero = np.zeros(self.image.shape[:2]) if self.debug: output_image = self.output_image.copy() counter = 0 occupied = 0.0 total = 0.0 occupied_map = {} for l in labels: label_indices = np.argwhere(ariadne.graph.labels == l) counter += label_indices.shape[0] occupied_map[l] = float(labels_count[l]) / \ float(label_indices.shape[0]) if occupied_map[l] > 0.1: occupied += labels_count[l] total += label_indices.shape[0] if self.debug: output_image[label_indices[:, 0], label_indices[:, 1]] = np.array([255, 255, 255 ]).astype(np.uint8) zero[label_indices[:, 0], label_indices[:, 1]] = np.array([255]).astype(np.uint8) if self.debug: output_image[nonzeroindices[:, 0], nonzeroindices[:, 1]] = np.array([255, 0, 255]).astype(np.uint8) zero[nonzeroindices[:, 0], nonzeroindices[:, 1]] = 0 counter -= nonzeroindices.shape[0] void_space_single = 0.0 for k, v in occupied_map.items(): void_space_single += v void_space_single /= float(labels.shape[0]) void_space = (float(total) - float(occupied)) / float(total) #print("VOID_SPACE", void_space, void_space_single) if self.debug: cv2.imshow("model", ren_rgb) cv2.imshow("image", output_image) cv2.imshow("zero", zero) c = cv2.waitKey(1) #count = counter - labels_variance*0.01 e = void_space print("COUNTER DIFFERENCE", void_space, void_space_single) # if count == 0: # count = np.inf # print("CURENT X", x, count) return e
# Visualization #1 #----------------------------------------------------------------------- # Load RGB image rgb_path = rgb_path_mask.format(device, scene_id, im_id, rgb_ext[device]) rgb = scipy.misc.imread(rgb_path) im_size = (rgb.shape[1], rgb.shape[0]) vis_rgb = np.zeros(rgb.shape, np.float) for gt in scene_gt[im_id]: model = models[gt['obj_id']] R = gt['cam_R_m2c'] t = gt['cam_t_m2c'] surf_color = obj_colors[gt['obj_id'] - 1] ren_rgb = renderer.render(model, im_size, K, R, t, surf_color=surf_color, mode='rgb') # Draw the bounding box of the object ren_rgb = misc.draw_rect(ren_rgb, gt['obj_bb']) vis_rgb += 0.7 * ren_rgb.astype(np.float) # Save the visualization vis_rgb = 0.6 * vis_rgb + 0.4 * rgb vis_rgb[vis_rgb > 255] = 255 vis_rgb_path = vis_rgb_path_mask.format(scene_id, device, model_type, im_id) scipy.misc.imsave(vis_rgb_path, vis_rgb.astype(np.uint8)) # Visualization #2 #----------------------------------------------------------------------- if device != 'canon':