def _visualize_affordance_map(self, preds, depth_im, scale, plot_max=True, output_dir=None): """Visualize an affordance map of the network predictions overlayed on the depth image.""" self._logger.info('Visualizing affordance map...') affordance_map = preds[0, ..., 0] tf_depth_im = depth_im.crop( depth_im.shape[0] - self._gqcnn_recep_h, depth_im.shape[1] - self._gqcnn_recep_w).resize(1.0 / self._gqcnn_stride) # plot vis.figure() vis.imshow(tf_depth_im) plt.imshow(affordance_map, cmap=plt.cm.RdYlGn, alpha=0.3, vmin=0.0, vmax=1.0) if plot_max: affordance_argmax = np.unravel_index(np.argmax(affordance_map), affordance_map.shape) plt.scatter(affordance_argmax[1], affordance_argmax[0], c='black', marker='.', s=scale * 25) vis.title('Grasp Affordance Map') if output_dir is not None: vis.savefig(os.path.join(output_dir, 'grasp_affordance_map.png')) else: vis.show()
def main(args): from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d # set logging logging.getLogger().setLevel(logging.INFO) rospy.init_node('ensenso_reader', anonymous=True) num_frames = 5 sensor = EnsensoSensor() sensor.start() total_time = 0 for i in range(num_frames): if i > 0: start_time = time.time() _, depth_im, _ = sensor.frames() if i > 0: total_time += time.time() - start_time logging.info('Frame %d' % (i)) logging.info('Avg FPS: %.5f' % (float(i) / total_time)) depth_im = sensor.median_depth_img(num_img=5) point_cloud = sensor.ir_intrinsics.deproject(depth_im) point_cloud.remove_zero_points() sensor.stop() vis2d.figure() vis2d.imshow(depth_im) vis2d.title('Ensenso - Raw') vis2d.show()
def main(args): # set logging logging.getLogger().setLevel(logging.INFO) rospy.init_node("ensenso_reader", anonymous=True) num_frames = 10 sensor = RgbdSensorFactory("ensenso", cfg={"frame": "ensenso"}) sensor.start() total_time = 0 for i in range(num_frames): if i > 0: start_time = time.time() _, depth_im, _ = sensor.frames() if i > 0: total_time += time.time() - start_time print("Frame %d" % (i)) print("Avg FPS: %.5f" % (float(i) / total_time)) depth_im = sensor.median_depth_img(num_img=5) point_cloud = sensor.ir_intrinsics.deproject(depth_im) point_cloud.remove_zero_points() sensor.stop() vis2d.figure() vis2d.imshow(depth_im) vis2d.title("Ensenso - Raw") vis2d.show() vis3d.figure() vis3d.points(point_cloud, random=True, subsample=10, scale=0.0025) vis3d.show()
def quality(self, state, actions, params): """Evaluate the quality of a set of actions according to a GQ-CNN. Parameters ---------- state : :obj:`RgbdImageState` State of the world described by an RGB-D image. actions: :obj:`object` Set of grasping actions to evaluate. params: dict Optional parameters for quality evaluation. Returns ------- :obj:`list` of float Real-valued grasp quality predictions for each action, between 0 and 1. """ # Form tensors. image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) if params is not None and params["vis"]["tf_images"]: # Read vis params. k = params["vis"]["k"] d = utils.sqrt_ceil(k) # Display grasp transformed images. from visualization import Visualizer2D as vis2d vis2d.figure(size=(GeneralConstants.FIGSIZE, GeneralConstants.FIGSIZE)) for i, image_tf in enumerate(image_tensor[:k, ...]): depth = pose_tensor[i][0] vis2d.subplot(d, d, i + 1) vis2d.imshow(DepthImage(image_tf)) vis2d.title("Image %d: d=%.3f" % (i, depth)) vis2d.show() # Predict grasps. num_actions = len(actions) null_arr = -1 * np.ones(self._batch_size) predict_start = time() output_arr = np.zeros([num_actions, 2]) cur_i = 0 end_i = cur_i + min(self._batch_size, num_actions - cur_i) while cur_i < num_actions: output_arr[cur_i:end_i, :] = self.gqcnn( image_tensor[cur_i:end_i, :, :, 0], pose_tensor[cur_i:end_i, 0], null_arr)[0] cur_i = end_i end_i = cur_i + min(self._batch_size, num_actions - cur_i) q_values = output_arr[:, -1] self._logger.debug("Prediction took %.3f sec" % (time() - predict_start)) return q_values.tolist()
def visualization(self, action, rgbd_im, offset, scale): # Vis final grasp. if self.policy_config["vis"]["final_grasp"]: vis.figure(size=(10, 10)) vis.imshow(rgbd_im.depth, vmin=self.policy_config["vis"]["vmin"], vmax=self.policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=True, show_axis=True) vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format( action.grasp.depth * scale + offset, action.q_value)) vis.savefig('test_dataset/grasp.png')
def quality(self, state, actions, params): """ Evaluate the quality of a set of actions according to a GQ-CNN. Parameters ---------- state : :obj:`RgbdImageState` state of the world described by an RGB-D image actions: :obj:`object` set of grasping actions to evaluate params: dict optional parameters for quality evaluation Returns ------- :obj:`list` of float real-valued grasp quality predictions for each action, between 0 and 1 """ # form tensors image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) if params is not None and params['vis']['tf_images']: # read vis params k = params['vis']['k'] d = utils.sqrt_ceil(k) # display grasp transformed images from visualization import Visualizer2D as vis2d vis2d.figure(size=(FIGSIZE, FIGSIZE)) for i, image_tf in enumerate(image_tensor[:k, ...]): depth = pose_tensor[i][0] vis2d.subplot(d, d, i + 1) vis2d.imshow(GrayscaleImage(image_tf)) vis2d.title('Image %d: d=%.3f' % (i, depth)) vis2d.show() # predict grasps num_actions = len(actions) null_arr = -1 * np.ones(self._batch_size) predict_start = time() output_arr = np.zeros([num_actions, 2]) cur_i = 0 end_i = cur_i + min(self._batch_size, num_actions - cur_i) while cur_i < num_actions: output_arr[cur_i:end_i, :] = self.gqcnn( image_tensor[cur_i:end_i, :, :, 0], pose_tensor[cur_i:end_i, 0], null_arr)[0] cur_i = end_i end_i = cur_i + min(self._batch_size, num_actions - cur_i) q_values = output_arr[:, -1] logging.debug('Prediction took %.3f sec' % (time() - predict_start)) return q_values.tolist()
def quality(self, state, actions, params): """Evaluate the quality of a set of actions according to a GQ-CNN. Parameters ---------- state : :obj:`RgbdImageState` State of the world described by an RGB-D image. actions: :obj:`object` Set of grasping actions to evaluate. params: dict Optional parameters for quality evaluation. Returns ------- :obj:`list` of float Real-valued grasp quality predictions for each action, between 0 and 1. """ # Form tensors. tensor_start = time() image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) self._logger.info("Image transformation took %.3f sec" % (time() - tensor_start)) if params is not None and params["vis"]["tf_images"]: # Read vis params. k = params["vis"]["k"] d = utils.sqrt_ceil(k) # Display grasp transformed images. from visualization import Visualizer2D as vis2d vis2d.figure(size=(GeneralConstants.FIGSIZE, GeneralConstants.FIGSIZE)) for i, image_tf in enumerate(image_tensor[:k, ...]): depth = pose_tensor[i][0] vis2d.subplot(d, d, i + 1) vis2d.imshow(DepthImage(image_tf)) vis2d.title("Image %d: d=%.3f" % (i, depth)) vis2d.show() # Predict grasps. predict_start = time() output_arr = self.gqcnn.predict(image_tensor, pose_tensor) q_values = output_arr[:, -1] self._logger.info("Inference took %.3f sec" % (time() - predict_start)) return q_values.tolist()
def quality(self, state, actions, params): """ Evaluate the quality of a set of actions according to a GQ-CNN. Parameters ---------- state : :obj:`RgbdImageState` state of the world described by an RGB-D image actions: :obj:`object` set of grasping actions to evaluate params: dict optional parameters for quality evaluation Returns ------- :obj:`list` of float real-valued grasp quality predictions for each action, between 0 and 1 """ # form tensors tensor_start = time() image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) logging.info('Image transformation took %.3f sec' % (time() - tensor_start)) if params is not None and params['vis']['tf_images']: # read vis params k = params['vis']['k'] d = utils.sqrt_ceil(k) # display grasp transformed images from visualization import Visualizer2D as vis2d vis2d.figure(size=(FIGSIZE, FIGSIZE)) for i, image_tf in enumerate(image_tensor[:k, ...]): depth = pose_tensor[i][0] vis2d.subplot(d, d, i + 1) vis2d.imshow(DepthImage(image_tf)) vis2d.title('Image %d: d=%.3f' % (i, depth)) vis2d.show() # predict grasps predict_start = time() output_arr = self.gqcnn.predict(image_tensor, pose_tensor) q_values = output_arr[:, -1] logging.info('Inference took %.3f sec' % (time() - predict_start)) return q_values.tolist()
def main(args): # set logging logging.getLogger().setLevel(logging.INFO) rospy.init_node('ensenso_reader', anonymous=True) num_frames = 10 #sensor = PhoXiSensor(frame='phoxi', # size='small') sensor = EnsensoSensor(frame='ensenso') sensor.start() total_time = 0 for i in range(num_frames): if i > 0: start_time = time.time() _, depth_im, _ = sensor.frames() if i > 0: total_time += time.time() - start_time print('Frame %d' % (i)) print('Avg FPS: %.5f' % (float(i) / total_time)) depth_im = sensor.median_depth_img(num_img=5) point_cloud = sensor.ir_intrinsics.deproject(depth_im) point_cloud.remove_zero_points() sensor.stop() vis2d.figure() vis2d.imshow(depth_im) vis2d.title('PhoXi - Raw') vis2d.show() vis3d.figure() vis3d.points(point_cloud, random=True, subsample=10, scale=0.0025) vis3d.show()
def _visualize_2d(self, actions, preds, wrapped_depth_im, num_actions, scale, show_axis, output_dir=None): """Visualize the actions in 2D.""" self._logger.info("Visualizing actions in 2d...") # Plot actions in 2D. vis.figure() vis.imshow(wrapped_depth_im) for i in range(num_actions): vis.grasp(actions[i].grasp, scale=scale, show_axis=show_axis, color=plt.cm.RdYlGn(actions[i].q_value)) vis.title("Top {} Grasps".format(num_actions)) if output_dir is not None: vis.savefig(os.path.join(output_dir, "top_grasps.png")) else: vis.show()
def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image by finding depth edges, then uniformly sampling point pairs and keeping only antipodal grasps with width less than the maximum allowable. Parameters ---------- depth_im : :obj:"perception.DepthImage" Depth image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` Intrinsics of the camera that captured the images. num_samples : int Number of grasps to sample. segmask : :obj:`perception.BinaryImage` Binary image segmenting out the object of interest. visualize : bool Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`Grasp2D` List of 2D grasp candidates. """ # Compute edge pixels. edge_start = time() depth_im = depth_im.apply(snf.gaussian_filter, sigma=self._depth_grad_gaussian_sigma) scale_factor = self._rescale_factor depth_im_downsampled = depth_im.resize(scale_factor) depth_im_threshed = depth_im_downsampled.threshold_gradients( self._depth_grad_thresh) edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels() edge_pixels = edge_pixels.astype(np.int16) depth_im_mask = depth_im.copy() if segmask is not None: edge_pixels = np.array( [p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)]) depth_im_mask = depth_im.mask_binary(segmask) # Re-threshold edges if there are too few. if edge_pixels.shape[0] < self._min_num_edge_pixels: self._logger.info("Too few edge pixels!") depth_im_threshed = depth_im.threshold_gradients( self._depth_grad_thresh) edge_pixels = depth_im_threshed.zero_pixels() edge_pixels = edge_pixels.astype(np.int16) depth_im_mask = depth_im.copy() if segmask is not None: edge_pixels = np.array([ p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0) ]) depth_im_mask = depth_im.mask_binary(segmask) num_pixels = edge_pixels.shape[0] self._logger.debug("Depth edge detection took %.3f sec" % (time() - edge_start)) self._logger.debug("Found %d edge pixels" % (num_pixels)) # Compute point cloud. point_cloud_im = camera_intr.deproject_to_image(depth_im_mask) # Compute_max_depth. depth_data = depth_im_mask.data[depth_im_mask.data > 0] if depth_data.shape[0] == 0: return [] min_depth = np.min(depth_data) + self._min_depth_offset max_depth = np.max(depth_data) + self._max_depth_offset # Compute surface normals. normal_start = time() edge_normals = self._surface_normals(depth_im, edge_pixels) self._logger.debug("Normal computation took %.3f sec" % (time() - normal_start)) if visualize: edge_pixels = edge_pixels[::2, :] edge_normals = edge_normals[::2, :] vis.figure() vis.subplot(1, 3, 1) vis.imshow(depth_im) if num_pixels > 0: vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b") X = [pix[1] for pix in edge_pixels] Y = [pix[0] for pix in edge_pixels] U = [3 * pix[1] for pix in edge_normals] V = [-3 * pix[0] for pix in edge_normals] plt.quiver(X, Y, U, V, units="x", scale=0.25, width=0.5, zorder=2, color="r") vis.title("Edge pixels and normals") vis.subplot(1, 3, 2) vis.imshow(depth_im_threshed) vis.title("Edge map") vis.subplot(1, 3, 3) vis.imshow(segmask) vis.title("Segmask") vis.show() # Exit if no edge pixels. if num_pixels == 0: return [] # Form set of valid candidate point pairs. pruning_start = time() max_grasp_width_px = Grasp2D(Point(np.zeros(2)), 0.0, min_depth, width=self._gripper_width, camera_intr=camera_intr).width_px normal_ip = edge_normals.dot(edge_normals.T) dists = ssd.squareform(ssd.pdist(edge_pixels)) valid_indices = np.where( (normal_ip < -np.cos(np.arctan(self._friction_coef))) & (dists < max_grasp_width_px) & (dists > 0.0)) valid_indices = np.c_[valid_indices[0], valid_indices[1]] self._logger.debug("Normal pruning %.3f sec" % (time() - pruning_start)) # Raise exception if no antipodal pairs. num_pairs = valid_indices.shape[0] if num_pairs == 0: return [] # Prune out grasps. contact_points1 = edge_pixels[valid_indices[:, 0], :] contact_points2 = edge_pixels[valid_indices[:, 1], :] contact_normals1 = edge_normals[valid_indices[:, 0], :] contact_normals2 = edge_normals[valid_indices[:, 1], :] v = contact_points1 - contact_points2 v_norm = np.linalg.norm(v, axis=1) v = v / np.tile(v_norm[:, np.newaxis], [1, 2]) ip1 = np.sum(contact_normals1 * v, axis=1) ip2 = np.sum(contact_normals2 * (-v), axis=1) ip1[ip1 > 1.0] = 1.0 ip1[ip1 < -1.0] = -1.0 ip2[ip2 > 1.0] = 1.0 ip2[ip2 < -1.0] = -1.0 beta1 = np.arccos(ip1) beta2 = np.arccos(ip2) alpha = np.arctan(self._friction_coef) antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0] # Raise exception if no antipodal pairs. num_pairs = antipodal_indices.shape[0] if num_pairs == 0: return [] sample_size = min(self._max_rejection_samples, num_pairs) grasp_indices = np.random.choice(antipodal_indices, size=sample_size, replace=False) self._logger.debug("Grasp comp took %.3f sec" % (time() - pruning_start)) # Compute grasps. sample_start = time() k = 0 grasps = [] while k < sample_size and len(grasps) < num_samples: grasp_ind = grasp_indices[k] p1 = contact_points1[grasp_ind, :] p2 = contact_points2[grasp_ind, :] n1 = contact_normals1[grasp_ind, :] n2 = contact_normals2[grasp_ind, :] # width = np.linalg.norm(p1 - p2) k += 1 # Compute center and axis. grasp_center = (p1 + p2) // 2 grasp_axis = p2 - p1 grasp_axis = grasp_axis / np.linalg.norm(grasp_axis) grasp_theta = np.pi / 2 if grasp_axis[1] != 0: grasp_theta = np.arctan2(grasp_axis[0], grasp_axis[1]) grasp_center_pt = Point(np.array( [grasp_center[1], grasp_center[0]]), frame=camera_intr.frame) # Compute grasp points in 3D. x1 = point_cloud_im[p1[0], p1[1]] x2 = point_cloud_im[p2[0], p2[1]] if np.linalg.norm(x2 - x1) > self._gripper_width: continue # Perturb. if self._grasp_center_sigma > 0.0: grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs( cov=self._grasp_center_sigma * np.diag(np.ones(2))) if self._grasp_angle_sigma > 0.0: grasp_theta = grasp_theta + ss.norm.rvs( scale=self._grasp_angle_sigma) # Check center px dist from boundary. if (grasp_center[0] < self._min_dist_from_boundary or grasp_center[1] < self._min_dist_from_boundary or grasp_center[0] > depth_im.height - self._min_dist_from_boundary or grasp_center[1] > depth_im.width - self._min_dist_from_boundary): continue # Sample depths. for i in range(self._depth_samples_per_grasp): # Get depth in the neighborhood of the center pixel. depth_win = depth_im.data[grasp_center[0] - self._h:grasp_center[0] + self._h, grasp_center[1] - self._w:grasp_center[1] + self._w] center_depth = np.min(depth_win) if center_depth == 0 or np.isnan(center_depth): continue # Sample depth between the min and max. min_depth = center_depth + self._min_depth_offset max_depth = center_depth + self._max_depth_offset sample_depth = min_depth + (max_depth - min_depth) * np.random.rand() candidate_grasp = Grasp2D(grasp_center_pt, grasp_theta, sample_depth, width=self._gripper_width, camera_intr=camera_intr, contact_points=[p1, p2], contact_normals=[n1, n2]) if visualize: vis.figure() vis.imshow(depth_im) vis.grasp(candidate_grasp) vis.scatter(p1[1], p1[0], c="b", s=25) vis.scatter(p2[1], p2[0], c="b", s=25) vis.show() grasps.append(candidate_grasp) # Return sampled grasps. self._logger.debug("Loop took %.3f sec" % (time() - sample_start)) return grasps
def _plot(self, model_dir, model_output_dir, train_result, val_result): """Plot analysis curves.""" self.logger.info("Plotting") _, model_name = os.path.split(model_output_dir) # Set params. colors = ["g", "b", "c", "y", "m", "r"] styles = ["-", "--", "-.", ":", "-"] # PR, ROC. vis2d.clf() train_result.precision_recall_curve(plot=True, line_width=self.line_width, color=colors[0], style=styles[0], label="TRAIN") val_result.precision_recall_curve(plot=True, line_width=self.line_width, color=colors[1], style=styles[1], label="VAL") vis2d.title("Precision Recall Curves", fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() vis2d.legend(handles, labels, loc="best") figname = os.path.join(model_output_dir, "precision_recall.png") vis2d.savefig(figname, dpi=self.dpi) vis2d.clf() train_result.roc_curve(plot=True, line_width=self.line_width, color=colors[0], style=styles[0], label="TRAIN") val_result.roc_curve(plot=True, line_width=self.line_width, color=colors[1], style=styles[1], label="VAL") vis2d.title("Reciever Operating Characteristic", fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() vis2d.legend(handles, labels, loc="best") figname = os.path.join(model_output_dir, "roc.png") vis2d.savefig(figname, dpi=self.dpi) # Plot histogram of prediction errors. num_bins = min(self.num_bins, train_result.num_datapoints) # Train positives. pos_ind = np.where(train_result.labels == 1)[0] diffs = np.abs(train_result.labels[pos_ind] - train_result.pred_probs[pos_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0, 1), normalized=False, plot=True) vis2d.title("Error on Positive Training Examples", fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) figname = os.path.join(model_output_dir, "pos_train_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Train negatives. neg_ind = np.where(train_result.labels == 0)[0] diffs = np.abs(train_result.labels[neg_ind] - train_result.pred_probs[neg_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0, 1), normalized=False, plot=True) vis2d.title("Error on Negative Training Examples", fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) figname = os.path.join(model_output_dir, "neg_train_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Histogram of validation errors. num_bins = min(self.num_bins, val_result.num_datapoints) # Val positives. pos_ind = np.where(val_result.labels == 1)[0] diffs = np.abs(val_result.labels[pos_ind] - val_result.pred_probs[pos_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0, 1), normalized=False, plot=True) vis2d.title("Error on Positive Validation Examples", fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) figname = os.path.join(model_output_dir, "pos_val_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Val negatives. neg_ind = np.where(val_result.labels == 0)[0] diffs = np.abs(val_result.labels[neg_ind] - val_result.pred_probs[neg_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0, 1), normalized=False, plot=True) vis2d.title("Error on Negative Validation Examples", fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) figname = os.path.join(model_output_dir, "neg_val_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Losses. try: train_errors_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_ERRORS) val_errors_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ERRORS) val_iters_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ITERS) pct_pos_val_filename = os.path.join(model_dir, GQCNNFilenames.PCT_POS_VAL) train_losses_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_LOSSES) raw_train_errors = np.load(train_errors_filename) val_errors = np.load(val_errors_filename) val_iters = np.load(val_iters_filename) pct_pos_val = float(val_errors[0]) if os.path.exists(pct_pos_val_filename): pct_pos_val = 100.0 * np.load(pct_pos_val_filename) raw_train_losses = np.load(train_losses_filename) val_errors = np.r_[pct_pos_val, val_errors] val_iters = np.r_[0, val_iters] # Window the training error. i = 0 train_errors = [] train_losses = [] train_iters = [] while i < raw_train_errors.shape[0]: train_errors.append(np.mean(raw_train_errors[i:i + WINDOW])) train_losses.append(np.mean(raw_train_losses[i:i + WINDOW])) train_iters.append(i) i += WINDOW train_errors = np.array(train_errors) train_losses = np.array(train_losses) train_iters = np.array(train_iters) init_val_error = val_errors[0] norm_train_errors = train_errors / init_val_error norm_val_errors = val_errors / init_val_error norm_final_val_error = val_result.error_rate / val_errors[0] if pct_pos_val > 0: norm_final_val_error = val_result.error_rate / pct_pos_val vis2d.clf() vis2d.plot(train_iters, train_errors, linewidth=self.line_width, color="b") vis2d.plot(val_iters, val_errors, linewidth=self.line_width, color="g") vis2d.ylim(0, 100) vis2d.legend(("TRAIN (Minibatch)", "VAL"), fontsize=self.font_size, loc="best") vis2d.xlabel("Iteration", fontsize=self.font_size) vis2d.ylabel("Error Rate", fontsize=self.font_size) vis2d.title("Error Rate vs Training Iteration", fontsize=self.font_size) figname = os.path.join(model_output_dir, "training_error_rates.png") vis2d.savefig(figname, dpi=self.dpi) vis2d.clf() vis2d.plot(train_iters, norm_train_errors, linewidth=4, color="b") vis2d.plot(val_iters, norm_val_errors, linewidth=4, color="g") vis2d.ylim(0, 2.0) vis2d.legend(("TRAIN (Minibatch)", "VAL"), fontsize=self.font_size, loc="best") vis2d.xlabel("Iteration", fontsize=self.font_size) vis2d.ylabel("Normalized Error Rate", fontsize=self.font_size) vis2d.title("Normalized Error Rate vs Training Iteration", fontsize=self.font_size) figname = os.path.join(model_output_dir, "training_norm_error_rates.png") vis2d.savefig(figname, dpi=self.dpi) train_losses[train_losses > MAX_LOSS] = MAX_LOSS # CAP LOSSES. vis2d.clf() vis2d.plot(train_iters, train_losses, linewidth=self.line_width, color="b") vis2d.ylim(0, 2.0) vis2d.xlabel("Iteration", fontsize=self.font_size) vis2d.ylabel("Loss", fontsize=self.font_size) vis2d.title("Training Loss vs Iteration", fontsize=self.font_size) figname = os.path.join(model_output_dir, "training_losses.png") vis2d.savefig(figname, dpi=self.dpi) # Log. self.logger.info("TRAIN") self.logger.info("Original error: %.3f" % (train_errors[0])) self.logger.info("Final error: %.3f" % (train_result.error_rate)) self.logger.info("Orig loss: %.3f" % (train_losses[0])) self.logger.info("Final loss: %.3f" % (train_losses[-1])) self.logger.info("VAL") self.logger.info("Original error: %.3f" % (pct_pos_val)) self.logger.info("Final error: %.3f" % (val_result.error_rate)) self.logger.info("Normalized error: %.3f" % (norm_final_val_error)) return (train_errors[0], train_result.error_rate, train_losses[0], train_losses[-1], pct_pos_val, val_result.error_rate, norm_final_val_error) except Exception as e: self.logger.error("Failed to plot training curves!\n" + str(e))
# load test case state_path = os.path.join(test_case_path, 'state') action_path = os.path.join(test_case_path, 'action') state = RgbdImageState.load(state_path) original_action = ParallelJawGrasp.load(action_path) # init policy policy = CrossEntropyRobustGraspingPolicy(policy_config) if policy_config['vis']['input_images']: vis2d.figure() if state.segmask is None: vis2d.subplot(1, 2, 1) vis2d.imshow(state.rgbd_im.color) vis2d.title('COLOR') vis2d.subplot(1, 2, 2) vis2d.imshow(state.rgbd_im.depth, vmin=policy_config['vis']['vmin'], vmax=policy_config['vis']['vmax']) vis2d.title('DEPTH') else: vis2d.subplot(1, 3, 1) vis2d.imshow(state.rgbd_im.color) vis2d.title('COLOR') vis2d.subplot(1, 3, 2) vis2d.imshow(state.rgbd_im.depth, vmin=policy_config['vis']['vmin'], vmax=policy_config['vis']['vmax']) vis2d.title('DEPTH') vis2d.subplot(1, 3, 3)
# preprocess color_im, depth_im, segmask = preprocess_images(raw_color_im, raw_depth_im, camera_intr, T_camera_world, workspace_box, workspace_im, image_proc_config) # visualize if vis: gui = plt.figure(0) plt.clf() vis2d.subplot(2,3,1) vis2d.imshow(raw_color_im) vis2d.title('RAW COLOR') vis2d.subplot(2,3,2) vis2d.imshow(raw_depth_im) vis2d.title('RAW DEPTH') vis2d.subplot(2,3,4) vis2d.imshow(color_im) vis2d.title('COLOR') vis2d.subplot(2,3,5) vis2d.imshow(depth_im) vis2d.title('DEPTH') vis2d.subplot(2,3,6) vis2d.imshow(segmask) vis2d.title('SEGMASK') plt.draw() plt.pause(GUI_PAUSE)
def _run_prediction_single_model(self, model_dir, model_output_dir, dataset_config): """Analyze the performance of a single model.""" # Read in model config. model_config_filename = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) with open(model_config_filename) as data_file: model_config = json.load(data_file) # Load model. self.logger.info("Loading model %s" % (model_dir)) log_file = None for handler in self.logger.handlers: if isinstance(handler, logging.FileHandler): log_file = handler.baseFilename gqcnn = get_gqcnn_model(verbose=self.verbose).load( model_dir, verbose=self.verbose, log_file=log_file) gqcnn.open_session() gripper_mode = gqcnn.gripper_mode angular_bins = gqcnn.angular_bins # Read params from the config. if dataset_config is None: dataset_dir = model_config["dataset_dir"] split_name = model_config["split_name"] image_field_name = model_config["image_field_name"] pose_field_name = model_config["pose_field_name"] metric_name = model_config["target_metric_name"] metric_thresh = model_config["metric_thresh"] else: dataset_dir = dataset_config["dataset_dir"] split_name = dataset_config["split_name"] image_field_name = dataset_config["image_field_name"] pose_field_name = dataset_config["pose_field_name"] metric_name = dataset_config["target_metric_name"] metric_thresh = dataset_config["metric_thresh"] gripper_mode = dataset_config["gripper_mode"] self.logger.info("Loading dataset %s" % (dataset_dir)) dataset = TensorDataset.open(dataset_dir) train_indices, val_indices, _ = dataset.split(split_name) # Visualize conv filters. conv1_filters = gqcnn.filters num_filt = conv1_filters.shape[3] d = utils.sqrt_ceil(num_filt) vis2d.clf() for k in range(num_filt): filt = conv1_filters[:, :, 0, k] vis2d.subplot(d, d, k + 1) vis2d.imshow(DepthImage(filt)) figname = os.path.join(model_output_dir, "conv1_filters.pdf") vis2d.savefig(figname, dpi=self.dpi) # Aggregate training and validation true labels and predicted # probabilities. all_predictions = [] if angular_bins > 0: all_predictions_raw = [] all_labels = [] for i in range(dataset.num_tensors): # Log progress. if i % self.log_rate == 0: self.logger.info("Predicting tensor %d of %d" % (i + 1, dataset.num_tensors)) # Read in data. image_arr = dataset.tensor(image_field_name, i).arr pose_arr = read_pose_data( dataset.tensor(pose_field_name, i).arr, gripper_mode) metric_arr = dataset.tensor(metric_name, i).arr label_arr = 1 * (metric_arr > metric_thresh) label_arr = label_arr.astype(np.uint8) if angular_bins > 0: # Form mask to extract predictions from ground-truth angular # bins. raw_poses = dataset.tensor(pose_field_name, i).arr angles = raw_poses[:, 3] neg_ind = np.where(angles < 0) # TODO(vsatish): These should use the max angle instead. angles = np.abs(angles) % GeneralConstants.PI angles[neg_ind] *= -1 g_90 = np.where(angles > (GeneralConstants.PI / 2)) l_neg_90 = np.where(angles < (-1 * (GeneralConstants.PI / 2))) angles[g_90] -= GeneralConstants.PI angles[l_neg_90] += GeneralConstants.PI # TODO(vsatish): Fix this along with the others. angles *= -1 # Hack to fix reverse angle convention. angles += (GeneralConstants.PI / 2) pred_mask = np.zeros((raw_poses.shape[0], angular_bins * 2), dtype=bool) bin_width = GeneralConstants.PI / angular_bins for i in range(angles.shape[0]): pred_mask[i, int((angles[i] // bin_width) * 2)] = True pred_mask[i, int((angles[i] // bin_width) * 2 + 1)] = True # Predict with GQ-CNN. predictions = gqcnn.predict(image_arr, pose_arr) if angular_bins > 0: raw_predictions = np.array(predictions) predictions = predictions[pred_mask].reshape((-1, 2)) # Aggregate. all_predictions.extend(predictions[:, 1].tolist()) if angular_bins > 0: all_predictions_raw.extend(raw_predictions.tolist()) all_labels.extend(label_arr.tolist()) # Close session. gqcnn.close_session() # Create arrays. all_predictions = np.array(all_predictions) all_labels = np.array(all_labels) train_predictions = all_predictions[train_indices] val_predictions = all_predictions[val_indices] train_labels = all_labels[train_indices] val_labels = all_labels[val_indices] if angular_bins > 0: all_predictions_raw = np.array(all_predictions_raw) train_predictions_raw = all_predictions_raw[train_indices] val_predictions_raw = all_predictions_raw[val_indices] # Aggregate results. train_result = BinaryClassificationResult(train_predictions, train_labels) val_result = BinaryClassificationResult(val_predictions, val_labels) train_result.save(os.path.join(model_output_dir, "train_result.cres")) val_result.save(os.path.join(model_output_dir, "val_result.cres")) # Get stats, plot curves. self.logger.info("Model %s training error rate: %.3f" % (model_dir, train_result.error_rate)) self.logger.info("Model %s validation error rate: %.3f" % (model_dir, val_result.error_rate)) self.logger.info("Model %s training loss: %.3f" % (model_dir, train_result.cross_entropy_loss)) self.logger.info("Model %s validation loss: %.3f" % (model_dir, val_result.cross_entropy_loss)) # Save images. vis2d.figure() example_dir = os.path.join(model_output_dir, "examples") if not os.path.exists(example_dir): os.mkdir(example_dir) # Train. self.logger.info("Saving training examples") train_example_dir = os.path.join(example_dir, "train") if not os.path.exists(train_example_dir): os.mkdir(train_example_dir) # Train TP. true_positive_indices = train_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( "Datapoint %d: Pred: %.3f Label: %.3f" % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, "true_positive_%03d.png" % (i))) # Train FP. false_positive_indices = train_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( "Datapoint %d: Pred: %.3f Label: %.3f" % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, "false_positive_%03d.png" % (i))) # Train TN. true_negative_indices = train_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( "Datapoint %d: Pred: %.3f Label: %.3f" % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, "true_negative_%03d.png" % (i))) # Train TP. false_negative_indices = train_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( "Datapoint %d: Pred: %.3f Label: %.3f" % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, "false_negative_%03d.png" % (i))) # Val. self.logger.info("Saving validation examples") val_example_dir = os.path.join(example_dir, "val") if not os.path.exists(val_example_dir): os.mkdir(val_example_dir) # Val TP. true_positive_indices = val_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, "true_positive_%03d.png" % (i))) # Val FP. false_positive_indices = val_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, "false_positive_%03d.png" % (i))) # Val TN. true_negative_indices = val_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, "true_negative_%03d.png" % (i))) # Val TP. false_negative_indices = val_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, "false_negative_%03d.png" % (i))) # Save summary stats. train_summary_stats = { "error_rate": train_result.error_rate, "ap_score": train_result.ap_score, "auc_score": train_result.auc_score, "loss": train_result.cross_entropy_loss } train_stats_filename = os.path.join(model_output_dir, "train_stats.json") json.dump(train_summary_stats, open(train_stats_filename, "w"), indent=JSON_INDENT, sort_keys=True) val_summary_stats = { "error_rate": val_result.error_rate, "ap_score": val_result.ap_score, "auc_score": val_result.auc_score, "loss": val_result.cross_entropy_loss } val_stats_filename = os.path.join(model_output_dir, "val_stats.json") json.dump(val_summary_stats, open(val_stats_filename, "w"), indent=JSON_INDENT, sort_keys=True) return train_result, val_result
def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image by finding depth edges, then uniformly sampling point pairs and keeping only antipodal grasps with width less than the maximum allowable. Parameters ---------- depth_im : :obj:"perception.DepthImage" Depth image to sample from. camera_intr : :obj:`autolab.CameraIntrinsics` Intrinsics of the camera that captured the images. num_samples : int Number of grasps to sample. segmask : :obj:`perception.BinaryImage` Binary image segmenting out the object of interest. visualize : bool Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`Grasp2D` List of 2D grasp candidates. """ # Compute edge pixels. edge_start = time() depth_im = depth_im.apply( snf. gaussian_filter, #Create a new image by applying a function to this image's data. sigma=self._depth_grad_gaussian_sigma) #这里会造成失真,最好用copy scale_factor = self._rescale_factor depth_im_downsampled = depth_im.resize( scale_factor) #Resize the image.#这里会造成失真,最好用copy depth_im_threshed = depth_im_downsampled.threshold_gradients( #Creates a new DepthImage by zeroing out all depths where the magnitude of the gradient at that point is greater than grad_thresh. self._depth_grad_thresh) edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels( ) #Return an array of the zero pixels. edge_pixels = edge_pixels.astype( np.int16) #找到所有的大于_depth_grad_thresh的像素点。 depth_im_mask = depth_im.copy() if segmask is not None: edge_pixels = np.array( [p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)]) depth_im_mask = depth_im.mask_binary(segmask) # Re-threshold edges if there are too few. if edge_pixels.shape[0] < self._min_num_edge_pixels: self._logger.info("Too few edge pixels!") depth_im_threshed = depth_im.threshold_gradients( self._depth_grad_thresh) edge_pixels = depth_im_threshed.zero_pixels( ) #Return an array of the zero pixels的坐标. edge_pixels = edge_pixels.astype( np.int16) #找到所有的大于_depth_grad_thresh的像素点。 depth_im_mask = depth_im.copy() # depth_im.resize(scale_factor) if segmask is not None: #如果有segmask,就去除segmask外的edge_pixels,以及深度信息。 edge_pixels = np.array([ p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0) ]) depth_im_mask = depth_im.mask_binary(segmask) num_pixels = edge_pixels.shape[0] #边缘像素的数量 self._logger.debug("Depth edge detection took %.3f sec" % (time() - edge_start)) self._logger.debug("Found %d edge pixels" % (num_pixels)) # Compute point cloud. point_cloud_im = camera_intr.deproject_to_image( depth_im_mask) # Deprojects a DepthImage into a PointCloudImage. # Compute_max_depth. depth_data = depth_im_mask.data[ depth_im_mask.data > 0] #depth_im_mask是指经过mask后的深度图(如果有mask) if depth_data.shape[0] == 0: #失败 return [] min_depth = np.min( depth_data ) + self._min_depth_offset # Offset from the minimum depth at the grasp center pixel to use in depth sampling max_depth = np.max(depth_data) + self._max_depth_offset # Compute surface normals. normal_start = time() edge_normals = self._surface_normals( depth_im, edge_pixels ) # Return an array of the surface normals at the edge pixels. self._logger.debug("Normal computation took %.3f sec" % (time() - normal_start)) if visualize: edge_pixels = edge_pixels[::2, :] edge_normals = edge_normals[::2, :] vis.figure() vis.subplot(1, 3, 1) vis.imshow(depth_im) if num_pixels > 0: vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b") X = [pix[1] for pix in edge_pixels] Y = [pix[0] for pix in edge_pixels] U = [3 * pix[1] for pix in edge_normals] V = [-3 * pix[0] for pix in edge_normals] plt.quiver(X, Y, U, V, units="x", scale=0.25, width=0.5, zorder=2, color="r") vis.title("Edge pixels and normals") vis.subplot(1, 3, 2) vis.imshow(depth_im_threshed) vis.title("Edge map") vis.subplot(1, 3, 3) vis.imshow(segmask) vis.title("Segmask") vis.show() # Exit if no edge pixels. if num_pixels == 0: return [] # Form set of valid candidate point pairs. #先找到一部分符合要求的抓取点。 pruning_start = time() max_grasp_width_px = Grasp2D( Point(np.zeros(2)), 0.0, min_depth, width=self._gripper_width, camera_intr=camera_intr).width_px #计算Returns the width in pixels. normal_ip = edge_normals.dot(edge_normals.T) #ab cos(θ)是一个对称矩阵 dists = ssd.squareform(ssd.pdist(edge_pixels)) # 是一个对称矩阵 #ssd.pdist(edge_pixels)计算每个点距离其他点之间的距离。ssd.squareform 用来把一个向量格式的距离向量转换成一个方阵格式的距离矩阵,反之亦然。 #ssd.pdist先形成距离函数,再由squareform形成一个距离方阵 #https://blog.csdn.net/qq_20135597/article/details/94212816?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control valid_indices = np.where( (normal_ip < -np.cos(np.arctan(self._friction_coef))) & (dists < max_grasp_width_px) & (dists > 0.0) ) #两个点之间符合force closure,距离小于max_grasp_width,同时两点距离大于零。np.where对于二维方阵生成的是两个array,是两个配对点的索引 valid_indices = np.c_[valid_indices[0], valid_indices[ 1]] #Translates slice objects to concatenation along the second axis.即将valid_indices[1]并到valid_indices[0]后面。 self._logger.debug("Normal pruning %.3f sec" % (time() - pruning_start)) # Raise exception if no antipodal pairs. num_pairs = valid_indices.shape[0] if num_pairs == 0: return [] # Prune out grasps. contact_points1 = edge_pixels[ valid_indices[:, 0], :] # valid_indices:[ [c1x,c1y] [c2x,c2y].. ] valid_indices所有的cnx所对应的那一行 也就是第一个点的坐标 contact_points2 = edge_pixels[ valid_indices[:, 1], :] # valid_indices:[ [c1x,c1y] [c2x,c2y].. ] valid_indices所有的cny所对应的那一行 也就是第二个点的坐标 contact_normals1 = edge_normals[valid_indices[:, 0], :] # contact_normals2 = edge_normals[valid_indices[:, 1], :] # v = contact_points1 - contact_points2 v_norm = np.linalg.norm(v, axis=1) v = v / np.tile(v_norm[:, np.newaxis], [1, 2]) ip1 = np.sum(contact_normals1 * v, axis=1) ip2 = np.sum(contact_normals2 * (-v), axis=1) ip1[ip1 > 1.0] = 1.0 ip1[ip1 < -1.0] = -1.0 ip2[ip2 > 1.0] = 1.0 ip2[ip2 < -1.0] = -1.0 beta1 = np.arccos(ip1) beta2 = np.arccos(ip2) alpha = np.arctan(self._friction_coef) antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0] # Raise exception if no antipodal pairs. num_pairs = antipodal_indices.shape[0] if num_pairs == 0: return [] sample_size = min(self._max_rejection_samples, num_pairs) #防止采样个数过多造成计算负担,设置一个最大的采样量。 grasp_indices = np.random.choice( antipodal_indices, #随机选择一部分抓取 size=sample_size, replace=False) self._logger.debug("Grasp comp took %.3f sec" % (time() - pruning_start)) # Compute grasps. sample_start = time() k = 0 grasps = [] while k < sample_size and len(grasps) < num_samples: grasp_ind = grasp_indices[k] p1 = contact_points1[grasp_ind, :] #第一个点的坐标 p2 = contact_points2[grasp_ind, :] #第二个点的坐标 n1 = contact_normals1[grasp_ind, :] n2 = contact_normals2[grasp_ind, :] # width = np.linalg.norm(p1 - p2) k += 1 depth_p1 = depth_im[p1[0], p1[1]] depth_p2 = depth_im[p2[0], p2[1]] # Compute center and axis. grasp_center = (p1 + p2) // 2 #一个抓取的中心 grasp_axis = p2 - p1 grasp_axis = grasp_axis / np.linalg.norm(grasp_axis) grasp_theta = np.pi / 2 if grasp_axis[1] != 0: grasp_theta = np.arctan2(grasp_axis[0], grasp_axis[1]) #一个抓取的轴角 grasp_center_pt = Point(np.array( [grasp_center[1], grasp_center[0]]), frame=camera_intr.frame) # Compute grasp points in 3D.只是为了确定两点距离小于夹爪宽度 x1 = point_cloud_im[p1[0], p1[1]] x2 = point_cloud_im[p2[0], p2[1]] if np.linalg.norm(x2 - x1) > self._gripper_width: continue # Perturb. if self._grasp_center_sigma > 0.0: grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs( cov=self._grasp_center_sigma * np.diag(np.ones(2))) if self._grasp_angle_sigma > 0.0: grasp_theta = grasp_theta + ss.norm.rvs( scale=self._grasp_angle_sigma) # Check center px dist from boundary. 检查抓取中心是否在工作空间内 if (grasp_center[0] < self._min_dist_from_boundary or grasp_center[1] < self._min_dist_from_boundary or grasp_center[0] > depth_im.height - self._min_dist_from_boundary or grasp_center[1] > depth_im.width - self._min_dist_from_boundary): continue depth_p1 = depth_im[p1[0], p1[1]] depth_p2 = depth_im[p2[0], p2[1]] depth_grasp = (depth_p1 + depth_p2) / 2 depth_grasp = depth_grasp[0] candidate_grasp = Grasp2D(grasp_center_pt, grasp_theta, depth_grasp, width=self._gripper_width, camera_intr=camera_intr, contact_points=[p1, p2], contact_normals=[n1, n2]) grasps.append(candidate_grasp) ''' for i in range(self._depth_samples_per_grasp): # Get depth in the neighborhood of the center pixel. depth_win = depth_im.data[grasp_center[0] - self._h:grasp_center[0] + self._h, grasp_center[1] - self._w:grasp_center[1] + self._w] center_depth = np.min(depth_win) if center_depth == 0 or np.isnan(center_depth): continue # Sample depth between the min and max. min_depth = center_depth + self._min_depth_offset max_depth = center_depth + self._max_depth_offset sample_depth = min_depth + (max_depth - min_depth) * np.random.rand() candidate_grasp = Grasp2D(grasp_center_pt, grasp_theta, sample_depth, width=self._gripper_width, camera_intr=camera_intr, contact_points=[p1, p2], contact_normals=[n1, n2]) if visualize: vis.figure() vis.imshow(depth_im) vis.grasp(candidate_grasp) vis.scatter(p1[1], p1[0], c="b", s=25) vis.scatter(p2[1], p2[0], c="b", s=25) vis.show() grasps.append(candidate_grasp) ''' # Return sampled grasps. self._logger.debug("Loop took %.3f sec" % (time() - sample_start)) return grasps
def _action(self, state): """ Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` image to plan grasps on Returns ------- :obj:`GraspAction` grasp to execute """ # check valid input if not isinstance(state, RgbdImageState): raise ValueError('Must provide an RGB-D image state.') # parse state seed_set_start = time() rgbd_im = state.rgbd_im depth_im = rgbd_im.depth camera_intr = state.camera_intr segmask = state.segmask point_cloud_im = camera_intr.deproject_to_image(depth_im) normal_cloud_im = point_cloud_im.normal_cloud_im() # sample grasps grasps = self._grasp_sampler.sample( rgbd_im, camera_intr, self._num_seed_samples, segmask=segmask, visualize=self.config['vis']['grasp_sampling'], seed=self._seed) num_grasps = len(grasps) if num_grasps == 0: logging.warning('No valid grasps could be found') raise NoValidGraspsException() grasp_type = 'parallel_jaw' if isinstance(grasps[0], SuctionPoint2D): grasp_type = 'suction' logging.info('Sampled %d grasps' % (len(grasps))) logging.info('Computing the seed set took %.3f sec' % (time() - seed_set_start)) # iteratively refit and sample for j in range(self._num_iters): logging.info('CEM iter %d' % (j)) # predict grasps predict_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) logging.info('Prediction took %.3f sec' % (time() - predict_start)) # sort grasps resample_start = time() q_values_and_indices = zip(q_values, np.arange(num_grasps)) q_values_and_indices.sort(key=lambda x: x[0], reverse=True) if self.config['vis']['grasp_candidates']: # display each grasp on the original image, colored by predicted success norm_q_values = q_values #(q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) vis.figure(size=(FIGSIZE, FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config['vis']['vmin'], vmax=self.config['vis']['vmax']) for grasp, q in zip(grasps, norm_q_values): vis.grasp(grasp, scale=2.0, jaw_width=2.0, show_center=False, show_axis=True, color=plt.cm.RdYlGn(q)) vis.title('Sampled grasps iter %d' % (j)) filename = None if self._logging_dir is not None: filename = os.path.join(self._logging_dir, 'cem_iter_%d.png' % (j)) vis.show(filename) # fit elite set elite_start = time() num_refit = max(int(np.ceil(self._gmm_refit_p * num_grasps)), 1) elite_q_values = [i[0] for i in q_values_and_indices[:num_refit]] elite_grasp_indices = [ i[1] for i in q_values_and_indices[:num_refit] ] elite_grasps = [grasps[i] for i in elite_grasp_indices] elite_grasp_arr = np.array([g.feature_vec for g in elite_grasps]) if self.config['vis']['elite_grasps']: # display each grasp on the original image, colored by predicted success norm_q_values = (elite_q_values - np.min(elite_q_values)) / ( np.max(elite_q_values) - np.min(elite_q_values)) vis.figure(size=(FIGSIZE, FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config['vis']['vmin'], vmax=self.config['vis']['vmax']) for grasp, q in zip(elite_grasps, norm_q_values): vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True, color=plt.cm.RdYlGn(q)) vis.title('Elite grasps iter %d' % (j)) filename = None if self._logging_dir is not None: filename = os.path.join(self._logging_dir, 'elite_set_iter_%d.png' % (j)) vis.show(filename) # normalize elite set elite_grasp_mean = np.mean(elite_grasp_arr, axis=0) elite_grasp_std = np.std(elite_grasp_arr, axis=0) elite_grasp_std[elite_grasp_std == 0] = 1e-6 elite_grasp_arr = (elite_grasp_arr - elite_grasp_mean) / elite_grasp_std logging.info('Elite set computation took %.3f sec' % (time() - elite_start)) # fit a GMM to the top samples num_components = max( int(np.ceil(self._gmm_component_frac * num_refit)), 1) uniform_weights = (1.0 / num_components) * np.ones(num_components) gmm = GaussianMixture(n_components=num_components, weights_init=uniform_weights, reg_covar=self._gmm_reg_covar) train_start = time() gmm.fit(elite_grasp_arr) logging.info('GMM fitting with %d components took %.3f sec' % (num_components, time() - train_start)) # sample the next grasps grasps = [] loop_start = time() num_tries = 0 while len( grasps ) < self._num_gmm_samples and num_tries < self._max_resamples_per_iteration: # sample from GMM sample_start = time() grasp_vecs, _ = gmm.sample(n_samples=self._num_gmm_samples) grasp_vecs = elite_grasp_std * grasp_vecs + elite_grasp_mean logging.info('GMM sampling took %.3f sec' % (time() - sample_start)) # convert features to grasps and store if in segmask for k, grasp_vec in enumerate(grasp_vecs): feature_start = time() if grasp_type == 'parallel_jaw': # form grasp object grasp = Grasp2D.from_feature_vec( grasp_vec, width=self._gripper_width, camera_intr=camera_intr) elif grasp_type == 'suction': # read depth and approach axis u = int(min(max(grasp_vec[1], 0), depth_im.height - 1)) v = int(min(max(grasp_vec[0], 0), depth_im.width - 1)) grasp_depth = depth_im[u, v] # approach_axis grasp_axis = -normal_cloud_im[u, v] # form grasp object grasp = SuctionPoint2D.from_feature_vec( grasp_vec, camera_intr=camera_intr, depth=grasp_depth, axis=grasp_axis) logging.debug('Feature vec took %.5f sec' % (time() - feature_start)) bounds_start = time() # check in bounds if state.segmask is None or \ (grasp.center.y >= 0 and grasp.center.y < state.segmask.height and \ grasp.center.x >= 0 and grasp.center.x < state.segmask.width and \ np.any(state.segmask[int(grasp.center.y), int(grasp.center.x)] != 0) and \ grasp.approach_angle < self._max_approach_angle): # check validity according to filters grasps.append(grasp) logging.debug('Bounds took %.5f sec' % (time() - bounds_start)) num_tries += 1 # check num grasps num_grasps = len(grasps) if num_grasps == 0: logging.warning('No valid grasps could be found') raise NoValidGraspsException() logging.info('Resample loop took %.3f sec' % (time() - loop_start)) logging.info('Resampling took %.3f sec' % (time() - resample_start)) # predict final set of grasps predict_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) logging.info('Final prediction took %.3f sec' % (time() - predict_start)) if self.config['vis']['grasp_candidates']: # display each grasp on the original image, colored by predicted success norm_q_values = q_values #(q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) vis.figure(size=(FIGSIZE, FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config['vis']['vmin'], vmax=self.config['vis']['vmax']) for grasp, q in zip(grasps, norm_q_values): vis.grasp(grasp, scale=2.0, jaw_width=2.0, show_center=False, show_axis=True, color=plt.cm.RdYlGn(q)) vis.title('Final sampled grasps') filename = None if self._logging_dir is not None: filename = os.path.join(self._logging_dir, 'final_grasps.png') vis.show(filename) # select grasp index = self.select(grasps, q_values) grasp = grasps[index] q_value = q_values[index] if self.config['vis']['grasp_plan']: vis.figure() vis.imshow(rgbd_im.depth, vmin=self.config['vis']['vmin'], vmax=self.config['vis']['vmax']) vis.grasp(grasp, scale=5.0, show_center=False, show_axis=True, jaw_width=1.0, grasp_axis_width=0.2) vis.title('Best Grasp: d=%.3f, q=%.3f' % (grasp.depth, q_value)) filename = None if self._logging_dir is not None: filename = os.path.join(self._logging_dir, 'planned_grasp.png') vis.show(filename) # form return image image = state.rgbd_im.depth if isinstance(self._grasp_quality_fn, GQCnnQualityFunction): image_arr, _ = self._grasp_quality_fn.grasps_to_tensors([grasp], state) image = DepthImage(image_arr[0, ...], frame=state.rgbd_im.frame) # return action action = GraspAction(grasp, q_value, image) return action
os.path.join(config["calib_dir"], sensor_frame, tf_filename)) # Setup sensor. sensor = RgbdSensorFactory.sensor(sensor_type, config["sensor"]) sensor.start() camera_intr = sensor.ir_intrinsics # Read images. color_im, depth_im, _ = sensor.frames() color_im = color_im.inpaint(rescale_factor=inpaint_rescale_factor) depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) # Sample grasps. grasp_sampler = AntipodalDepthImageGraspSampler(sample_config, gripper_width) grasps = grasp_sampler.sample(rgbd_im, camera_intr, num_grasp_samples, segmask=None, seed=100, visualize=visualize_sampling) # Visualize. vis.figure() vis.imshow(depth_im) for grasp in grasps: vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True) vis.title("Sampled grasps") vis.show()
grasp.angle, grasp.depth, width=gripper_width, camera_intr=camera_intr) elif grasp_type == GQCNNGrasp.SUCTION: center = Point(np.array([grasp.center_px[0], grasp.center_px[1]]), frame=camera_intr.frame) grasp_2d = SuctionPoint2D(center, np.array([0, 0, 1]), grasp.depth, camera_intr=camera_intr) else: raise ValueError('Grasp type %d not recognized!' % (grasp_type)) try: thumbnail = DepthImage(cv_bridge.imgmsg_to_cv2( grasp.thumbnail, desired_encoding="passthrough"), frame=camera_intr.frame) except CVBridgeError as e: logging.error(e) logging.error('Failed to convert image') sys.exit(1) action = GraspAction(grasp_2d, grasp.q_value, thumbnail) # vis final grasp if vis_grasp: vis.figure(size=(10, 10)) vis.imshow(depth_im, vmin=0.6, vmax=0.9) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) vis.title('Planned grasp on depth (Q=%.3f)' % (action.q_value)) vis.show()
raw_color_im, raw_depth_im, camera_intr, T_camera_world, workspace_box, workspace_im, image_proc_config, ) # visualize if vis: gui = plt.figure(0) plt.clf() vis2d.subplot(2, 3, 1) vis2d.imshow(raw_color_im) vis2d.title("RAW COLOR") vis2d.subplot(2, 3, 2) vis2d.imshow(raw_depth_im) vis2d.title("RAW DEPTH") vis2d.subplot(2, 3, 4) vis2d.imshow(color_im) vis2d.title("COLOR") vis2d.subplot(2, 3, 5) vis2d.imshow(depth_im) vis2d.title("DEPTH") vis2d.subplot(2, 3, 6) vis2d.imshow(segmask) vis2d.title("SEGMASK") plt.draw() plt.pause(GUI_PAUSE)
def _plot(self, model_dir, model_output_dir, train_result, val_result): """ Plot analysis curves """ self.logger.info('Plotting') _, model_name = os.path.split(model_output_dir) # set params colors = ['g', 'b', 'c', 'y', 'm', 'r'] styles = ['-', '--', '-.', ':', '-'] num_colors = len(colors) num_styles = len(styles) # PR, ROC vis2d.clf() train_result.precision_recall_curve(plot=True, line_width=self.line_width, color=colors[0], style=styles[0], label='TRAIN') val_result.precision_recall_curve(plot=True, line_width=self.line_width, color=colors[1], style=styles[1], label='VAL') vis2d.title('Precision Recall Curves', fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() vis2d.legend(handles, labels, loc='best') figname = os.path.join(model_output_dir, 'precision_recall.png') vis2d.savefig(figname, dpi=self.dpi) vis2d.clf() train_result.roc_curve(plot=True, line_width=self.line_width, color=colors[0], style=styles[0], label='TRAIN') val_result.roc_curve(plot=True, line_width=self.line_width, color=colors[1], style=styles[1], label='VAL') vis2d.title('Reciever Operating Characteristic', fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() vis2d.legend(handles, labels, loc='best') figname = os.path.join(model_output_dir, 'roc.png') vis2d.savefig(figname, dpi=self.dpi) # plot histogram of prediction errors num_bins = min(self.num_bins, train_result.num_datapoints) # train positives pos_ind = np.where(train_result.labels == 1)[0] diffs = np.abs(train_result.labels[pos_ind] - train_result.pred_probs[pos_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0,1), normalized=False, plot=True) vis2d.title('Error on Positive Training Examples', fontsize=self.font_size) vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) vis2d.ylabel('Count', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'pos_train_errors_histogram.png') vis2d.savefig(figname, dpi=self.dpi) # train negatives neg_ind = np.where(train_result.labels == 0)[0] diffs = np.abs(train_result.labels[neg_ind] - train_result.pred_probs[neg_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0,1), normalized=False, plot=True) vis2d.title('Error on Negative Training Examples', fontsize=self.font_size) vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) vis2d.ylabel('Count', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'neg_train_errors_histogram.png') vis2d.savefig(figname, dpi=self.dpi) # histogram of validation errors num_bins = min(self.num_bins, val_result.num_datapoints) # val positives pos_ind = np.where(val_result.labels == 1)[0] diffs = np.abs(val_result.labels[pos_ind] - val_result.pred_probs[pos_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0,1), normalized=False, plot=True) vis2d.title('Error on Positive Validation Examples', fontsize=self.font_size) vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) vis2d.ylabel('Count', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'pos_val_errors_histogram.png') vis2d.savefig(figname, dpi=self.dpi) # val negatives neg_ind = np.where(val_result.labels == 0)[0] diffs = np.abs(val_result.labels[neg_ind] - val_result.pred_probs[neg_ind]) vis2d.figure() utils.histogram(diffs, num_bins, bounds=(0,1), normalized=False, plot=True) vis2d.title('Error on Negative Validation Examples', fontsize=self.font_size) vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) vis2d.ylabel('Count', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'neg_val_errors_histogram.png') vis2d.savefig(figname, dpi=self.dpi) # losses try: train_errors_filename = os.path.join(model_dir, TRAIN_ERRORS_FILENAME) val_errors_filename = os.path.join(model_dir, VAL_ERRORS_FILENAME) train_iters_filename = os.path.join(model_dir, TRAIN_ITERS_FILENAME) val_iters_filename = os.path.join(model_dir, VAL_ITERS_FILENAME) pct_pos_val_filename = os.path.join(model_dir, PCT_POS_VAL_FILENAME) train_losses_filename = os.path.join(model_dir, TRAIN_LOSS_FILENAME) raw_train_errors = np.load(train_errors_filename) val_errors = np.load(val_errors_filename) raw_train_iters = np.load(train_iters_filename) val_iters = np.load(val_iters_filename) pct_pos_val = float(val_errors[0]) if os.path.exists(pct_pos_val_filename): pct_pos_val = 100.0 * np.load(pct_pos_val_filename) raw_train_losses = np.load(train_losses_filename) val_errors = np.r_[pct_pos_val, val_errors] val_iters = np.r_[0, val_iters] # window the training error i = 0 train_errors = [] train_losses = [] train_iters = [] while i < raw_train_errors.shape[0]: train_errors.append(np.mean(raw_train_errors[i:i+WINDOW])) train_losses.append(np.mean(raw_train_losses[i:i+WINDOW])) train_iters.append(i) i += WINDOW train_errors = np.array(train_errors) train_losses = np.array(train_losses) train_iters = np.array(train_iters) init_val_error = val_errors[0] norm_train_errors = train_errors / init_val_error norm_val_errors = val_errors / init_val_error norm_final_val_error = val_result.error_rate / val_errors[0] if pct_pos_val > 0: norm_final_val_error = val_result.error_rate / pct_pos_val vis2d.clf() vis2d.plot(train_iters, train_errors, linewidth=self.line_width, color='b') vis2d.plot(val_iters, val_errors, linewidth=self.line_width, color='g') vis2d.ylim(0, 100) vis2d.legend(('TRAIN (Minibatch)', 'VAL'), fontsize=self.font_size, loc='best') vis2d.xlabel('Iteration', fontsize=self.font_size) vis2d.ylabel('Error Rate', fontsize=self.font_size) vis2d.title('Error Rate vs Training Iteration', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'training_error_rates.png') vis2d.savefig(figname, dpi=self.dpi) vis2d.clf() vis2d.plot(train_iters, norm_train_errors, linewidth=4, color='b') vis2d.plot(val_iters, norm_val_errors, linewidth=4, color='g') vis2d.ylim(0, 2.0) vis2d.legend(('TRAIN (Minibatch)', 'VAL'), fontsize=self.font_size, loc='best') vis2d.xlabel('Iteration', fontsize=self.font_size) vis2d.ylabel('Normalized Error Rate', fontsize=self.font_size) vis2d.title('Normalized Error Rate vs Training Iteration', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'training_norm_error_rates.png') vis2d.savefig(figname, dpi=self.dpi) train_losses[train_losses > MAX_LOSS] = MAX_LOSS # CAP LOSSES vis2d.clf() vis2d.plot(train_iters, train_losses, linewidth=self.line_width, color='b') vis2d.ylim(0, 2.0) vis2d.xlabel('Iteration', fontsize=self.font_size) vis2d.ylabel('Loss', fontsize=self.font_size) vis2d.title('Training Loss vs Iteration', fontsize=self.font_size) figname = os.path.join(model_output_dir, 'training_losses.png') vis2d.savefig(figname, dpi=self.dpi) # log self.logger.info('TRAIN') self.logger.info('Original error: %.3f' %(train_errors[0])) self.logger.info('Final error: %.3f' %(train_result.error_rate)) self.logger.info('Orig loss: %.3f' %(train_losses[0])) self.logger.info('Final loss: %.3f' %(train_losses[-1])) self.logger.info('VAL') self.logger.info('Original error: %.3f' %(pct_pos_val)) self.logger.info('Final error: %.3f' %(val_result.error_rate)) self.logger.info('Normalized error: %.3f' %(norm_final_val_error)) return train_errors[0], train_result.error_rate, train_losses[0], train_losses[-1], pct_pos_val, val_result.error_rate, norm_final_val_error except Exception as e: self.logger.error('Failed to plot training curves!\n' + str(e))
def _run_prediction_single_model(self, model_dir, model_output_dir, dataset_config): """ Analyze the performance of a single model. """ # read in model config model_config_filename = os.path.join(model_dir, 'config.json') with open(model_config_filename) as data_file: model_config = json.load(data_file) # load model logging.info('Loading model %s' % (model_dir)) gqcnn = GQCNN.load(model_dir) gqcnn.open_session() gripper_mode = gqcnn.gripper_mode # read params from the config if dataset_config is None: dataset_dir = model_config['dataset_dir'] split_name = model_config['split_name'] image_field_name = model_config['image_field_name'] pose_field_name = model_config['pose_field_name'] metric_name = model_config['target_metric_name'] metric_thresh = model_config['metric_thresh'] else: dataset_dir = dataset_config['dataset_dir'] split_name = dataset_config['split_name'] image_field_name = dataset_config['image_field_name'] pose_field_name = dataset_config['pose_field_name'] metric_name = dataset_config['target_metric_name'] metric_thresh = dataset_config['metric_thresh'] gripper_mode = dataset_config['gripper_mode'] logging.info('Loading dataset %s' % (dataset_dir)) dataset = TensorDataset.open(dataset_dir) train_indices, val_indices, _ = dataset.split(split_name) # visualize conv filters conv1_filters = gqcnn.filters num_filt = conv1_filters.shape[3] d = utils.sqrt_ceil(num_filt) vis2d.clf() for k in range(num_filt): filt = conv1_filters[:, :, 0, k] vis2d.subplot(d, d, k + 1) vis2d.imshow(DepthImage(filt)) figname = os.path.join(model_output_dir, 'conv1_filters.pdf') vis2d.savefig(figname, dpi=self.dpi) # aggregate training and validation true labels and predicted probabilities all_predictions = [] all_labels = [] for i in range(dataset.num_tensors): # log progress if i % self.log_rate == 0: logging.info('Predicting tensor %d of %d' % (i + 1, dataset.num_tensors)) # read in data image_arr = dataset.tensor(image_field_name, i).arr pose_arr = read_pose_data( dataset.tensor(pose_field_name, i).arr, gripper_mode) metric_arr = dataset.tensor(metric_name, i).arr label_arr = 1 * (metric_arr > metric_thresh) label_arr = label_arr.astype(np.uint8) # predict with GQ-CNN predictions = gqcnn.predict(image_arr, pose_arr) # aggregate all_predictions.extend(predictions[:, 1].tolist()) all_labels.extend(label_arr.tolist()) # close session gqcnn.close_session() # create arrays all_predictions = np.array(all_predictions) all_labels = np.array(all_labels) train_predictions = all_predictions[train_indices] val_predictions = all_predictions[val_indices] train_labels = all_labels[train_indices] val_labels = all_labels[val_indices] # aggregate results train_result = BinaryClassificationResult(train_predictions, train_labels) val_result = BinaryClassificationResult(val_predictions, val_labels) train_result.save(os.path.join(model_output_dir, 'train_result.cres')) val_result.save(os.path.join(model_output_dir, 'val_result.cres')) # get stats, plot curves logging.info('Model %s training error rate: %.3f' % (model_dir, train_result.error_rate)) logging.info('Model %s validation error rate: %.3f' % (model_dir, val_result.error_rate)) # save images vis2d.figure() example_dir = os.path.join(model_output_dir, 'examples') if not os.path.exists(example_dir): os.mkdir(example_dir) # train logging.info('Saving training examples') train_example_dir = os.path.join(example_dir, 'train') if not os.path.exists(train_example_dir): os.mkdir(train_example_dir) # train TP true_positive_indices = train_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( 'Datapoint %d: Pred: %.3f Label: %.3f' % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, 'true_positive_%03d.png' % (i))) # train FP false_positive_indices = train_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( 'Datapoint %d: Pred: %.3f Label: %.3f' % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, 'false_positive_%03d.png' % (i))) # train TN true_negative_indices = train_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( 'Datapoint %d: Pred: %.3f Label: %.3f' % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, 'true_negative_%03d.png' % (i))) # train TP false_negative_indices = train_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = train_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title( 'Datapoint %d: Pred: %.3f Label: %.3f' % (k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(train_example_dir, 'false_negative_%03d.png' % (i))) # val logging.info('Saving validation examples') val_example_dir = os.path.join(example_dir, 'val') if not os.path.exists(val_example_dir): os.mkdir(val_example_dir) # val TP true_positive_indices = val_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, 'true_positive_%03d.png' % (i))) # val FP false_positive_indices = val_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, 'false_positive_%03d.png' % (i))) # val TN true_negative_indices = val_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, 'true_negative_%03d.png' % (i))) # val TP false_negative_indices = val_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = val_indices[j] datapoint = dataset.datapoint( k, field_names=[image_field_name, pose_field_name]) vis2d.clf() self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' % (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig( os.path.join(val_example_dir, 'false_negative_%03d.png' % (i))) # save summary stats train_summary_stats = { 'error_rate': train_result.error_rate, 'ap_score': train_result.ap_score, 'auc_score': train_result.auc_score } train_stats_filename = os.path.join(model_output_dir, 'train_stats.json') json.dump(train_summary_stats, open(train_stats_filename, 'w'), indent=JSON_INDENT, sort_keys=True) val_summary_stats = { 'error_rate': val_result.error_rate, 'ap_score': val_result.ap_score, 'auc_score': val_result.auc_score } val_stats_filename = os.path.join(model_output_dir, 'val_stats.json') json.dump(val_summary_stats, open(val_stats_filename, 'w'), indent=JSON_INDENT, sort_keys=True) return train_result, val_result
grasp = grasp_policy(rgbd_state) #%% #print(grasp.grasp.center.x) #print(grasp.grasp.center.y) #%% vis.figure(size=(16, 16)) vis.imshow(rgbd_im.color, vmin=0.5, vmax=2.5) vis.grasp(grasp.grasp, scale=2.0, jaw_width=2.0, show_center=True, show_axis=True, color=plt.cm.RdYlBu(.1)) vis.title('Depth: ' + str(grasp.grasp.depth) + 'm ; Elite grasp with score: ' + str(grasp.q_value)) vis.show() #%% vis.figure(size=(16, 16)) vis.imshow(rgbd_im.depth, vmin=0.0, vmax=2.5) vis.grasp(grasp.grasp, scale=2.0, jaw_width=2.0, show_center=True, show_axis=True, color=plt.cm.RdYlBu(.1)) vis.title('Depth: ' + str(grasp.grasp.depth) + 'm ; Elite grasp with score: ' + str(grasp.q_value)) vis.show() #%%
def _action(self, state): """ Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` image to plan grasps on Returns ------- :obj:`GraspAction` grasp to execute """ # take the greedy action with prob 1 - epsilon if np.random.rand() > self.epsilon: logging.debug('Taking greedy action') return CrossEntropyRobustGraspingPolicy.action(self, state) # otherwise take a random action logging.debug('Taking random action') # check valid input if not isinstance(state, RgbdImageState): raise ValueError('Must provide an RGB-D image state.') # parse state rgbd_im = state.rgbd_im camera_intr = state.camera_intr segmask = state.segmask # sample random antipodal grasps grasps = self._grasp_sampler.sample( rgbd_im, camera_intr, self._num_seed_samples, segmask=segmask, visualize=self.config['vis']['grasp_sampling'], seed=self._seed) num_grasps = len(grasps) if num_grasps == 0: logging.warning('No valid grasps could be found') raise NoValidGraspsException() # choose a grasp uniformly at random grasp_ind = np.random.choice(num_grasps, size=1)[0] grasp = grasps[grasp_ind] depth = grasp.depth # create transformed image image_tensor, pose_tensor = self.grasps_to_tensors([grasp], state) image = DepthImage(image_tensor[0, ...]) # predict prob success output_arr = self.gqcnn.predict(image_tensor, pose_tensor) q_value = output_arr[0, -1] # visualize planned grasp if self.config['vis']['grasp_plan']: scale_factor = float(self.gqcnn.im_width) / float(self._crop_width) scaled_camera_intr = camera_intr.resize(scale_factor) vis_grasp = Grasp2D(Point(image.center), 0.0, depth, width=self._gripper_width, camera_intr=scaled_camera_intr) vis.figure() vis.imshow(image) vis.grasp(vis_grasp, scale=1.5, show_center=False, show_axis=True) vis.title('Best Grasp: d=%.3f, q=%.3f' % (depth, q_value)) vis.show() # return action return GraspAction(grasp, q_value, image)
else: raise ValueError('Invalid policy type: {}'.format(policy_type)) pathDataset = "/home/lvianell/Desktop/Lorenzo_report/datasets/dexnet_maskcnn_human/" #creo dataset: pngCounter = 0 #len(glob.glob1(pathDataset,"*.txt")) # save depth image in /home/lvianell/Desktop/gqcnn/data/dataset_human_demostration vis.figure(size=(10, 10)) vis.imshow(rgbd_im.depth) #, #vmin=policy_config['vis']['vmin'], #vmax=policy_config['vis']['vmax']) #vis.savefig(pathDataset+"depth_im_"+str(pngCounter)) vis.savefig(path_depth_png) vis.title("DepthImage") vis.show() print path_grasp_position text_file = open( path_grasp_position, "w") #open(pathDataset+"grasps_"+str(pngCounter)+".txt", "w") text_file.write("[center_x, center_y]" + "\t" + "angle" + "\t" + "depth" + "\t" + "width" + "\t" + "Qvalue" + "\t" + "human_evaluation" + "\n") flag_human = "y" #raw_input("Do you want to use human contribute? [y/N]") # query policy policy_start = time.time() if flag_human == "y":
#%% grasp = grasp_policy(rgbd_state) #%% img2 = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) img2 = cv2.circle(img2,(int(grasp.grasp.center.x),int(grasp.grasp.center.y)),2,(255,0,0),3) plt.imshow(img2) #%% img3 = cv2.cvtColor(d, cv2.COLOR_BGR2RGB) img3 = cv2.circle(img3,(int(grasp.grasp.center.x),int(grasp.grasp.center.y)),2,(255,0,0),3) plt.imshow(img3) #%% vis.figure(size=(16,16)) vis.imshow(rgbd_im.color, vmin=0.5, vmax=2.5) vis.grasp(grasp.grasp, scale=2.0,jaw_width=2.0, show_center=True, show_axis=True, color=plt.cm.RdYlBu(.1)) vis.title('Elite grasp with score: ' + str(grasp.q_value)) vis.show() #%% vis.figure(size=(16,16)) vis.imshow(rgbd_im.depth, vmin=0.5, vmax=2.5) vis.grasp(grasp.grasp, scale=2.0,jaw_width=2.0, show_center=True, show_axis=True, color=plt.cm.RdYlBu(.1)) vis.title('Elite grasp with score: ' + str(grasp.q_value)) vis.show() #%% print(grasp.grasp.angle) print(grasp.grasp.depth) print(grasp.grasp.width) print(grasp.grasp.axis) #%% grasp.grasp.approach_angle #%%
else: raise ValueError( "Invalid fully-convolutional policy type: {}".format( policy_config["type"])) else: policy_type = "cem" if "type" in policy_config: policy_type = policy_config["type"] if policy_type == "ranking": policy = RobustGraspingPolicy(policy_config) elif policy_type == "cem": policy = CrossEntropyRobustGraspingPolicy(policy_config) else: raise ValueError("Invalid policy type: {}".format(policy_type)) # Query policy. policy_start = time.time() action = policy(state) logger.info("Planning took %.3f sec" % (time.time() - policy_start)) # Vis final grasp. if policy_config["vis"]["final_grasp"]: vis.figure(size=(10, 10)) vis.imshow(rgbd_im.depth, vmin=policy_config["vis"]["vmin"], vmax=policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format( action.grasp.depth, action.q_value)) vis.show()
def execute_policy(self, rgbd_image_state, resetting=False): """ Executes a grasping policy on an `RgbdImageState`. Parameters ---------- rgbd_image_state: type `gqcnn.RgbdImageState` The :py:class:`gqcnn.RgbdImageState` that encapsulates the depth and color image along with camera intrinsics. """ policy_start = time.time() if not self.grasping_policy: self._get_grasp_policy() try: grasping_action = self.grasping_policy(rgbd_image_state) except: vis.figure(size=(10, 10)) vis.imshow(self.rgbd_im.color, vmin=0, vmax=255) vis.title("No Valid Grasp, Task Finished") vis.show() self.logger.info("Planning took %.3f sec" % (time.time() - policy_start)) # Angle of grasping point w.r.t the x-axis of camera frame angle_wrt_x = grasping_action.grasp.angle angle_degree = angle_wrt_x * 180 / np.pi if angle_degree <= -270: angle_degree += 360 elif (angle_degree > -270 and angle_degree <= -180) or (angle_degree > -180 and angle_degree <= -90): angle_degree += 180 elif (angle_degree > 90 and angle_degree <= 180) or (angle_degree > 180 and angle_degree <= 270): angle_degree -= 180 elif (angle_degree > 270 and angle_degree <= 360): angle_degree -= 360 angle_wrt_x = angle_degree * np.pi / 180 if resetting: angle_wrt_x += np.pi / 2 # Translation of grasping point w.r.t the camera frame grasping_translation = np.array([ grasping_action.grasp.pose().translation[0] * -1, grasping_action.grasp.pose().translation[1], grasping_action.grasp.pose().translation[2] * -1 ]) # Rotation matrix from world frame to camera frame world_to_cam_rotation = np.dot( np.array([[1, 0, 0], [0, np.cos(np.pi), -np.sin(np.pi)], [0, np.sin(np.pi), np.cos(np.pi)]]), np.array([[np.cos(np.pi), -np.sin(np.pi), 0], [np.sin(np.pi), np.cos(np.pi), 0], [0, 0, 1]])) # Rotation matrix from camera frame to gripper frame cam_to_gripper_rotation = np.array( [[np.cos(angle_wrt_x), -np.sin(angle_wrt_x), 0], [np.sin(angle_wrt_x), np.cos(angle_wrt_x), 0], [0, 0, 1]]) else: # Translation of grasping point w.r.t the camera frame grasping_translation = np.array([ grasping_action.grasp.pose().translation[1], grasping_action.grasp.pose().translation[0], grasping_action.grasp.pose().translation[2] ]) * -1 # Rotation matrix from world frame to camera frame world_to_cam_rotation = np.dot( np.array([[1, 0, 0], [0, np.cos(np.pi), -np.sin(np.pi)], [0, np.sin(np.pi), np.cos(np.pi)]]), np.array([[np.cos(np.pi / 2), -np.sin(np.pi / 2), 0], [np.sin(np.pi / 2), np.cos(np.pi / 2), 0], [0, 0, 1]])) # Rotation matrix from camera frame to gripper frame cam_to_gripper_rotation = np.dot( np.array([[np.cos(angle_wrt_x), -np.sin(angle_wrt_x), 0], [np.sin(angle_wrt_x), np.cos(angle_wrt_x), 0], [0, 0, 1]]), np.array([[np.cos(np.pi / 2), -np.sin(np.pi / 2), 0], [np.sin(np.pi / 2), np.cos(np.pi / 2), 0], [0, 0, 1]])) world_to_gripper_rotation = np.dot(world_to_cam_rotation, cam_to_gripper_rotation) quat_wxyz = from_rotation_matrix(world_to_gripper_rotation) grasping_quaternion = np.array( [quat_wxyz.x, quat_wxyz.y, quat_wxyz.z, quat_wxyz.w]) grasping_pose = np.hstack((grasping_translation, grasping_quaternion)) vis.figure(size=(10, 10)) vis.imshow(self.rgbd_im.color, vmin=0, vmax=255) vis.grasp(grasping_action.grasp, scale=2.5, show_center=False, show_axis=True) vis.title("Planned grasp at depth {0:.3f}m \n".format( grasping_action.grasp.depth) + 'grasping pose {}'.format(grasping_pose)) vis.show() return grasping_pose
def _run_prediction_single_model(self, model_dir, model_output_dir, dataset_config): """ Analyze the performance of a single model. """ # read in model config model_config_filename = os.path.join(model_dir, 'config.json') with open(model_config_filename) as data_file: model_config = json.load(data_file) # load model self.logger.info('Loading model %s' %(model_dir)) log_file = None for handler in self.logger.handlers: if isinstance(handler, logging.FileHandler): log_file = handler.baseFilename gqcnn = get_gqcnn_model(verbose=self.verbose).load(model_dir, verbose=self.verbose, log_file=log_file) gqcnn.open_session() gripper_mode = gqcnn.gripper_mode angular_bins = gqcnn.angular_bins # read params from the config if dataset_config is None: dataset_dir = model_config['dataset_dir'] split_name = model_config['split_name'] image_field_name = model_config['image_field_name'] pose_field_name = model_config['pose_field_name'] metric_name = model_config['target_metric_name'] metric_thresh = model_config['metric_thresh'] else: dataset_dir = dataset_config['dataset_dir'] split_name = dataset_config['split_name'] image_field_name = dataset_config['image_field_name'] pose_field_name = dataset_config['pose_field_name'] metric_name = dataset_config['target_metric_name'] metric_thresh = dataset_config['metric_thresh'] gripper_mode = dataset_config['gripper_mode'] self.logger.info('Loading dataset %s' %(dataset_dir)) dataset = TensorDataset.open(dataset_dir) train_indices, val_indices, _ = dataset.split(split_name) # visualize conv filters conv1_filters = gqcnn.filters num_filt = conv1_filters.shape[3] d = utils.sqrt_ceil(num_filt) vis2d.clf() for k in range(num_filt): filt = conv1_filters[:,:,0,k] vis2d.subplot(d,d,k+1) vis2d.imshow(DepthImage(filt)) figname = os.path.join(model_output_dir, 'conv1_filters.pdf') vis2d.savefig(figname, dpi=self.dpi) # aggregate training and validation true labels and predicted probabilities all_predictions = [] if angular_bins > 0: all_predictions_raw = [] all_labels = [] for i in range(dataset.num_tensors): # log progress if i % self.log_rate == 0: self.logger.info('Predicting tensor %d of %d' %(i+1, dataset.num_tensors)) # read in data image_arr = dataset.tensor(image_field_name, i).arr pose_arr = read_pose_data(dataset.tensor(pose_field_name, i).arr, gripper_mode) metric_arr = dataset.tensor(metric_name, i).arr label_arr = 1 * (metric_arr > metric_thresh) label_arr = label_arr.astype(np.uint8) if angular_bins > 0: # form mask to extract predictions from ground-truth angular bins raw_poses = dataset.tensor(pose_field_name, i).arr angles = raw_poses[:, 3] neg_ind = np.where(angles < 0) angles = np.abs(angles) % GeneralConstants.PI angles[neg_ind] *= -1 g_90 = np.where(angles > (GeneralConstants.PI / 2)) l_neg_90 = np.where(angles < (-1 * (GeneralConstants.PI / 2))) angles[g_90] -= GeneralConstants.PI angles[l_neg_90] += GeneralConstants.PI angles *= -1 # hack to fix reverse angle convention angles += (GeneralConstants.PI / 2) pred_mask = np.zeros((raw_poses.shape[0], angular_bins*2), dtype=bool) bin_width = GeneralConstants.PI / angular_bins for i in range(angles.shape[0]): pred_mask[i, int((angles[i] // bin_width)*2)] = True pred_mask[i, int((angles[i] // bin_width)*2 + 1)] = True # predict with GQ-CNN predictions = gqcnn.predict(image_arr, pose_arr) if angular_bins > 0: raw_predictions = np.array(predictions) predictions = predictions[pred_mask].reshape((-1, 2)) # aggregate all_predictions.extend(predictions[:,1].tolist()) if angular_bins > 0: all_predictions_raw.extend(raw_predictions.tolist()) all_labels.extend(label_arr.tolist()) # close session gqcnn.close_session() # create arrays all_predictions = np.array(all_predictions) all_labels = np.array(all_labels) train_predictions = all_predictions[train_indices] val_predictions = all_predictions[val_indices] train_labels = all_labels[train_indices] val_labels = all_labels[val_indices] if angular_bins > 0: all_predictions_raw = np.array(all_predictions_raw) train_predictions_raw = all_predictions_raw[train_indices] val_predictions_raw = all_predictions_raw[val_indices] # aggregate results train_result = BinaryClassificationResult(train_predictions, train_labels) val_result = BinaryClassificationResult(val_predictions, val_labels) train_result.save(os.path.join(model_output_dir, 'train_result.cres')) val_result.save(os.path.join(model_output_dir, 'val_result.cres')) # get stats, plot curves self.logger.info('Model %s training error rate: %.3f' %(model_dir, train_result.error_rate)) self.logger.info('Model %s validation error rate: %.3f' %(model_dir, val_result.error_rate)) self.logger.info('Model %s training loss: %.3f' %(model_dir, train_result.cross_entropy_loss)) self.logger.info('Model %s validation loss: %.3f' %(model_dir, val_result.cross_entropy_loss)) # save images vis2d.figure() example_dir = os.path.join(model_output_dir, 'examples') if not os.path.exists(example_dir): os.mkdir(example_dir) # train self.logger.info('Saving training examples') train_example_dir = os.path.join(example_dir, 'train') if not os.path.exists(train_example_dir): os.mkdir(train_example_dir) # train TP true_positive_indices = train_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = train_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(train_example_dir, 'true_positive_%03d.png' %(i))) # train FP false_positive_indices = train_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = train_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(train_example_dir, 'false_positive_%03d.png' %(i))) # train TN true_negative_indices = train_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = train_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(train_example_dir, 'true_negative_%03d.png' %(i))) # train TP false_negative_indices = train_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = train_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(train_example_dir, 'false_negative_%03d.png' %(i))) # val self.logger.info('Saving validation examples') val_example_dir = os.path.join(example_dir, 'val') if not os.path.exists(val_example_dir): os.mkdir(val_example_dir) # val TP true_positive_indices = val_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = val_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(val_example_dir, 'true_positive_%03d.png' %(i))) # val FP false_positive_indices = val_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = val_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(val_example_dir, 'false_positive_%03d.png' %(i))) # val TN true_negative_indices = val_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = val_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(val_example_dir, 'true_negative_%03d.png' %(i))) # val TP false_negative_indices = val_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = val_indices[j] datapoint = dataset.datapoint(k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) vis2d.savefig(os.path.join(val_example_dir, 'false_negative_%03d.png' %(i))) # save summary stats train_summary_stats = { 'error_rate': train_result.error_rate, 'ap_score': train_result.ap_score, 'auc_score': train_result.auc_score, 'loss': train_result.cross_entropy_loss } train_stats_filename = os.path.join(model_output_dir, 'train_stats.json') json.dump(train_summary_stats, open(train_stats_filename, 'w'), indent=JSON_INDENT, sort_keys=True) val_summary_stats = { 'error_rate': val_result.error_rate, 'ap_score': val_result.ap_score, 'auc_score': val_result.auc_score, 'loss': val_result.cross_entropy_loss } val_stats_filename = os.path.join(model_output_dir, 'val_stats.json') json.dump(val_summary_stats, open(val_stats_filename, 'w'), indent=JSON_INDENT, sort_keys=True) return train_result, val_result
def _action(self, state): """ Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` image to plan grasps on Returns ------- :obj:`GraspAction` grasp to execute """ # check valid input if not isinstance(state, RgbdImageState): raise ValueError('Must provide an RGB-D image state.') # parse state rgbd_im = state.rgbd_im camera_intr = state.camera_intr segmask = state.segmask # sample grasps grasps = self._grasp_sampler.sample( rgbd_im, camera_intr, self._num_grasp_samples, segmask=segmask, visualize=self.config['vis']['grasp_sampling'], seed=None) num_grasps = len(grasps) if num_grasps == 0: logging.warning('No valid grasps could be found') raise NoValidGraspsException() # compute grasp quality compute_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) logging.debug('Grasp evaluation took %.3f sec' % (time() - compute_start)) if self.config['vis']['grasp_candidates']: # display each grasp on the original image, colored by predicted success norm_q_values = (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) vis.figure(size=(FIGSIZE, FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config['vis']['vmin'], vmax=self.config['vis']['vmax']) for grasp, q in zip(grasps, norm_q_values): vis.grasp(grasp, scale=1.0, grasp_center_size=10, grasp_center_thickness=2.5, jaw_width=2.5, show_center=False, show_axis=True, color=plt.cm.RdYlGn(q)) vis.title('Sampled grasps') self.show('grasp_candidates.png') # select grasp index = self.select(grasps, q_values) grasp = grasps[index] q_value = q_values[index] if self.config['vis']['grasp_plan']: vis.figure() vis.imshow(rgbd_im.depth, vmin=self.config['vis']['vmin'], vmax=self.config['vis']['vmax']) vis.grasp(grasp, scale=2.0, show_axis=True) vis.title('Best Grasp: d=%.3f, q=%.3f' % (grasp.depth, q_value)) vis.show() return GraspAction(grasp, q_value, state.rgbd_im.depth)