def inpaint_depth_image(d_img, ix=0, iy=0): """Inpaint depth image on raw depth values. Only import code here to avoid making them required if we're not inpainting. Also, inpainting is slow, so crop some irrelevant values. But BE CAREFUL! Make sure any cropping here will lead to logical consistency with the processing in `camera.process_img_for_net` later. For now we crop the 'later part' of each dimension, which still leads to > 2x speed-up. The window size is 3 which I think means we can get away with a pixel difference of 3 when cropping but to be safe let's add a bit more, 50 pix to each side. For `ix` and `iy` see `camera.process_img_for_net`, makes inpainting faster. """ d_img = d_img[ix:685, iy:1130] from perception import (ColorImage, DepthImage) print('now in-painting the depth image (shape {}), ix, iy = {}, {}...'. format(d_img.shape, ix, iy)) start_t = time.time() d_img = DepthImage(d_img) d_img = d_img.inpaint() # inpaint, then get d_img right away d_img = d_img.data # get raw data back from the class cum_t = time.time() - start_t print('finished in-painting in {:.2f} seconds'.format(cum_t)) return d_img
def test_depth_init(self): # valid data random_valid_data = np.random.rand(IM_HEIGHT, IM_WIDTH).astype(np.float32) im = DepthImage(random_valid_data) self.assertEqual(im.height, IM_HEIGHT) self.assertEqual(im.width, IM_WIDTH) self.assertEqual(im.channels, 1) self.assertEqual(im.type, np.float32) self.assertTrue(np.allclose(im.data, random_valid_data)) # invalid channels random_data = np.random.rand(IM_HEIGHT, IM_WIDTH, 3).astype(np.float32) caught_bad_channels = False try: im = DepthImage(random_data) except: caught_bad_channels = True self.assertTrue(caught_bad_channels) # invalid type random_data = np.random.rand(IM_HEIGHT, IM_WIDTH).astype(np.uint8) caught_bad_dtype = False try: im = DepthImage(random_data) except: caught_bad_dtype = True self.assertTrue(caught_bad_dtype)
def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): """Generate the actions to be returned.""" depth_im = DepthImage(images[0], frame=camera_intr.frame) point_cloud_im = camera_intr.deproject_to_image(depth_im) normal_cloud_im = point_cloud_im.normal_cloud_im() actions = [] for i in range(num_actions): im_idx = ind[i, 0] h_idx = ind[i, 1] w_idx = ind[i, 2] center = Point( np.asarray([ w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2 ])) axis = -normal_cloud_im[center.y, center.x] if np.linalg.norm(axis) == 0: continue depth = depth_im[center.y, center.x, 0] if depth == 0.0: continue grasp = SuctionPoint2D(center, axis=axis, depth=depth, camera_intr=camera_intr) grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, 0], DepthImage(images[im_idx])) actions.append(grasp_action) return actions
def analyze_image_depths(path, bbox, out_name): """ path should lead to a .npy file """ img = np.load(path) img = np.reshape(img, img.shape[:2]) img_slice = img[bbox[0]:bbox[2], bbox[1]:bbox[3]] vec = np.ndarray.flatten(img_slice) # vec = reject_outliers(vec) var = np.var(vec) mean = np.mean(vec) print("State for {}: Mean: {}, Standard Deviation: {}\n".format( out_name, mean, np.sqrt(var))) n, bins, patches = plt.hist(vec, vec.size // 10, facecolor="blue") plt.xlabel("depth value") plt.ylabel("count") plt.title("depth within region") plt.grid(True) plt.show() plt.savefig(os.path.join(out_path, "graph_" + out_name), bbox_inches="tight") plt.close() depth_img = DepthImage(img_slice) depth_img.save(os.path.join(out_path, out_name))
def plan_grasp(self, depth, rgb, resetting=False, camera_intr=None, segmask=None): """ Computes possible grasps. Parameters ---------- depth: type `numpy` depth image rgb: type `numpy` rgb image camera_intr: type `perception.CameraIntrinsics` Intrinsic camera object. segmask: type `perception.BinaryImage` Binary segmask of detected object Returns ------- type `GQCNNGrasp` Computed optimal grasp. """ if "FC" in self.model: assert not (segmask is None), "Fully-Convolutional policy expects a segmask." if camera_intr is None: camera_intr_filename = os.path.join( os.path.dirname(os.path.realpath(__file__)), "gqcnn/data/calib/primesense/primesense.intr") camera_intr = CameraIntrinsics.load(camera_intr_filename) depth_im = DepthImage(depth, frame=camera_intr.frame) color_im = ColorImage(rgb, frame=camera_intr.frame) valid_px_mask = depth_im.invalid_pixel_mask().inverse() if segmask is None: segmask = valid_px_mask else: segmask = segmask.mask_binary(valid_px_mask) # Inpaint. depth_im = depth_im.inpaint( rescale_factor=self.config["inpaint_rescale_factor"]) # Aggregate color and depth images into a single # BerkeleyAutomation/perception `RgbdImage`. self.rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) # Create an `RgbdImageState` with the `RgbdImage` and `CameraIntrinsics`. state = RgbdImageState(self.rgbd_im, camera_intr, segmask=segmask) # Set input sizes for fully-convolutional policy. if "FC" in self.model: self.policy_config["metric"]["fully_conv_gqcnn_config"][ "im_height"] = depth_im.shape[0] self.policy_config["metric"]["fully_conv_gqcnn_config"][ "im_width"] = depth_im.shape[1] return self.execute_policy(state, resetting)
def inpaint(img): """ Inpaint the image """ # create DepthImage from gray version of img gray_img = skimage.color.rgb2gray(img) depth_img = DepthImage(gray_img) # zero out high-gradient areas and inpaint thresh_img = depth_img.threshold_gradients_pctile(0.95) inpaint_img = thresh_img.inpaint() return inpaint_img.data
def depth_to_bin(img_file): # Load the image as a numpy array and the camera intrinsics image = np.load(img_file) # Create and deproject a depth image of the data using the camera intrinsics di = DepthImage(image, frame=ci.frame) di = di.inpaint() bi = di.to_binary(threshold=0.85) #vis2d.figure() #vis2d.imshow(di) #vis2d.imshow(bi) #vis2d.show() return bi
def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): """Generate the actions to be returned.""" actions = [] # TODO(vsatish): These should use the max angle instead. ang_bin_width = GeneralConstants.PI / preds.shape[-1] for i in range(num_actions): im_idx = ind[i, 0] h_idx = ind[i, 1] w_idx = ind[i, 2] ang_idx = ind[i, 3] center = Point( np.asarray([ w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2 ])) ang = GeneralConstants.PI / 2 - (ang_idx * ang_bin_width + ang_bin_width / 2) depth = depths[im_idx, 0] grasp = Grasp2D(center, ang, depth, width=self._gripper_width, camera_intr=camera_intr) grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, ang_idx], DepthImage(images[im_idx])) actions.append(grasp_action) return actions
def _read_color_and_depth_image(self): """Read a color and depth image from the device. """ frames = self._pipe.wait_for_frames() if self._depth_align: frames = self._align.process(frames) depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: logging.warning('Could not retrieve frames.') return None, None if self._filter_depth: depth_frame = self._filter_depth_frame(depth_frame) # convert to numpy arrays depth_image = self._to_numpy(depth_frame, np.float32) color_image = self._to_numpy(color_frame, np.uint8) # convert depth to meters depth_image *= self._depth_scale # bgr to rgb color_image = color_image[..., ::-1] depth = DepthImage(depth_image, frame=self._frame) color = ColorImage(color_image, frame=self._frame) return color, depth
def largest_planar_surface(filename): # Load the image as a numpy array and the camera intrinsics image = np.load(filename) # Create and deproject a depth image of the data using the camera intrinsics di = DepthImage(image, frame=ci.frame) di = di.inpaint() pc = ci.deproject(di) # Make a PCL type point cloud from the image p = pcl.PointCloud(pc.data.T.astype(np.float32)) # Make a segmenter and segment the point cloud. seg = p.make_segmenter() seg.set_model_type(pcl.SACMODEL_PARALLEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) seg.set_distance_threshold(0.005) indices, model = seg.segment() return indices, model, image, pc
def frames(self): """Retrieve the next frame from the image directory and convert it to a ColorImage, a DepthImage, and an IrImage. Parameters ---------- skip_registration : bool If True, the registration step is skipped. Returns ------- :obj:`tuple` of :obj:`ColorImage`, :obj:`DepthImage`, :obj:`IrImage`, :obj:`numpy.ndarray` The ColorImage, DepthImage, and IrImage of the current frame. Raises ------ RuntimeError If the Kinect stream is not running or if all images in the directory have been used. """ if not self._running: raise RuntimeError('Primesense device pointing to %s not runnning. Cannot read frames' %(self._path_to_images)) if self._im_index > self._num_images: raise RuntimeError('Primesense device is out of images') # read images color_filename = os.path.join(self._path_to_images, 'color_%d.png' %(self._im_index)) color_im = ColorImage.open(color_filename, frame=self._frame) depth_filename = os.path.join(self._path_to_images, 'depth_%d.npy' %(self._im_index)) depth_im = DepthImage.open(depth_filename, frame=self._frame) self._im_index += 1 return color_im, depth_im, None
def _plot_grasp(self, datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=None): """ Plots a single grasp represented as a datapoint. """ image = DepthImage(datapoint[image_field_name][:,:,0]) depth = datapoint[pose_field_name][2] width = 0 grasps = [] if gripper_mode == GripperMode.PARALLEL_JAW or \ gripper_mode == GripperMode.LEGACY_PARALLEL_JAW: if angular_preds is not None: num_bins = angular_preds.shape[0] / 2 bin_width = GeneralConstants.PI / num_bins for i in range(num_bins): bin_cent_ang = i * bin_width + bin_width / 2 grasps.append(Grasp2D(center=image.center, angle=GeneralConstants.PI / 2 - bin_cent_ang, depth=depth, width=0.0)) grasps.append(Grasp2D(center=image.center, angle=datapoint[pose_field_name][3], depth=depth, width=0.0)) else: grasps.append(Grasp2D(center=image.center, angle=0, depth=depth, width=0.0)) width = datapoint[pose_field_name][-1] else: grasps.append(SuctionPoint2D(center=image.center, axis=[1,0,0], depth=depth)) vis2d.imshow(image) for i, grasp in enumerate(grasps[:-1]): vis2d.grasp(grasp, width=width, color=plt.cm.RdYlGn(angular_preds[i * 2 + 1])) vis2d.grasp(grasps[-1], width=width, color='b')
def load(save_dir): if not os.path.exists(save_dir): raise ValueError('Directory %s does not exist!' % (save_dir)) color_image_filename = os.path.join(save_dir, 'color.png') depth_image_filename = os.path.join(save_dir, 'depth.npy') camera_intr_filename = os.path.join(save_dir, 'camera.intr') segmask_filename = os.path.join(save_dir, 'segmask.npy') obj_segmask_filename = os.path.join(save_dir, 'obj_segmask.npy') state_filename = os.path.join(save_dir, 'state.pkl') camera_intr = CameraIntrinsics.load(camera_intr_filename) color = ColorImage.open(color_image_filename, frame=camera_intr.frame) depth = DepthImage.open(depth_image_filename, frame=camera_intr.frame) segmask = None if os.path.exists(segmask_filename): segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame) obj_segmask = None if os.path.exists(obj_segmask_filename): obj_segmask = SegmentationImage.open(obj_segmask_filename, frame=camera_intr.frame) fully_observed = None if os.path.exists(state_filename): fully_observed = pkl.load(open(state_filename, 'rb')) return RgbdImageState(RgbdImage.from_color_and_depth(color, depth), camera_intr, segmask=segmask, obj_segmask=obj_segmask, fully_observed=fully_observed)
def rendered_images(data, render_mode=RenderMode.SEGMASK): rendered_images = [] num_images = data.attrs[NUM_IMAGES_KEY] for i in range(num_images): # get the image data y'all image_key = IMAGE_KEY + '_' + str(i) image_data = data[image_key] image_arr = np.array(image_data[IMAGE_DATA_KEY]) frame = image_data.attrs[IMAGE_FRAME_KEY] if render_mode == RenderMode.SEGMASK: image = BinaryImage(image_arr, frame) elif render_mode == RenderMode.DEPTH: image = DepthImage(image_arr, frame) elif render_mode == RenderMode.SCALED_DEPTH: image = ColorImage(image_arr, frame) R_camera_table = image_data.attrs[CAM_ROT_KEY] t_camera_table = image_data.attrs[CAM_POS_KEY] frame = image_data.attrs[CAM_FRAME_KEY] T_camera_world = RigidTransform(R_camera_table, t_camera_table, from_frame=frame, to_frame='world') rendered_images.append(ObjectRender(image, T_camera_world)) return rendered_images
def get_state(self, depth, segmask): # Read images. depth_im = DepthImage(depth, frame=self.camera_intr.frame) color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), frame=self.camera_intr.frame) segmask = BinaryImage(segmask.astype(np.uint8) * 255, frame=self.camera_intr.frame) # Inpaint. depth_im = depth_im.inpaint(rescale_factor=self.inpaint_rescale_factor) # Create state. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state = RgbdImageState(rgbd_im, self.camera_intr, segmask=segmask) return state, rgbd_im
def load(save_dir): if not os.path.exists(save_dir): raise ValueError('Directory %s does not exist!' % (save_dir)) grasp_filename = os.path.join(save_dir, 'grasp.pkl') q_value_filename = os.path.join(save_dir, 'pred_robustness.pkl') image_filename = os.path.join(save_dir, 'tf_image.npy') grasp = pkl.load(open(grasp_filename, 'rb')) q_value = pkl.load(open(q_value_filename, 'rb')) image = DepthImage.open(image_filename) return GraspAction(grasp, q_value, image)
def _read_depth_images(self, num_images): """ Reads depth images from the device """ depth_images = self._ros_read_images(self._depth_image_buffer, num_images, self.staleness_limit) for i in range(0, num_images): depth_images[i] = depth_images[i] * MM_TO_METERS # convert to meters if self._flip_images: depth_images[i] = np.flipud(depth_images[i]) depth_images[i] = np.fliplr(depth_images[i]) depth_images[i] = DepthImage(depth_images[i], frame=self._frame) return depth_images
def _read_single_image(self, dataset, dataset_dir, datapoint): tensor = datapoint // dataset.datapoints_per_file array = datapoint % dataset.datapoints_per_file pointer = np.load( dataset_dir + '/tensors/image_files_{:05d}.npz'.format(tensor))['arr_0'][array] image = DepthImage( np.load(dataset_dir + '/images/depth_im_{:07d}.npy'.format(pointer))[:, :, 0]) return image
def read_images(self, req): """Reads images from a ROS service request. Parameters --------- req: :obj:`ROS ServiceRequest` ROS ServiceRequest for grasp planner service. """ # Get the raw depth and color images as ROS `Image` objects. raw_color = req.color_image raw_depth = req.depth_image # Get the raw camera info as ROS `CameraInfo`. raw_camera_info = req.camera_info # Wrap the camera info in a BerkeleyAutomation/perception # `CameraIntrinsics` object. camera_intr = CameraIntrinsics( raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) # Create wrapped BerkeleyAutomation/perception RGB and depth images by # unpacking the ROS images using ROS `CvBridge` try: color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2( raw_color, "rgb8"), frame=camera_intr.frame) depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2( raw_depth, desired_encoding="passthrough"), frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) # Check image sizes. if color_im.height != depth_im.height or \ color_im.width != depth_im.width: msg = ("Color image and depth image must be the same shape! Color" " is %d x %d but depth is %d x %d") % ( color_im.height, color_im.width, depth_im.height, depth_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) if (color_im.height < self.min_height or color_im.width < self.min_width): msg = ("Color image is too small! Must be at least %d x %d" " resolution but the requested image is only %d x %d") % ( self.min_height, self.min_width, color_im.height, color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) return color_im, depth_im, camera_intr
def visualize_predictions(run_dir, test_config, pred_mask_dir, pred_info_dir, show_bbox=True, show_class=True): """Visualizes predictions.""" # Create subdirectory for prediction visualizations vis_dir = os.path.join(run_dir, 'vis') depth_dir = os.path.join(test_config['path'], test_config['images']) if not os.path.exists(vis_dir): os.makedirs(vis_dir) indices_arr = np.load( os.path.join(test_config['path'], test_config['indices'])) image_ids = np.arange(indices_arr.size) ################################################################## # Process each image ################################################################## print('VISUALIZING PREDICTIONS') for image_id in tqdm(image_ids): depth_image_fn = 'image_{:06d}.npy'.format(indices_arr[image_id]) # depth_image_fn = 'image_{:06d}.png'.format(indices_arr[image_id]) # Load image and ground truth data and resize for net depth_data = np.load(os.path.join(depth_dir, depth_image_fn)) # image = ColorImage.open(os.path.join(depth_dir, '..', 'depth_ims', depth_image_fn)).data image = DepthImage(depth_data).to_color().data # load mask and info r = np.load( os.path.join(pred_info_dir, 'image_{:06}.npy'.format(image_id))).item() r_masks = np.load( os.path.join(pred_mask_dir, 'image_{:06}.npy'.format(image_id))) # Must transpose from (n, h, w) to (h, w, n) if r_masks.any(): r['masks'] = np.transpose(r_masks, (1, 2, 0)) else: r['masks'] = r_masks # Visualize fig = plt.figure(figsize=(1.7067, 1.7067), dpi=300, frameon=False) ax = plt.Axes(fig, [0., 0., 1., 1.]) fig.add_axes(ax) visualize.display_instances(image, r['rois'], r['masks'], r['class_ids'], ['bg', 'obj'], ax=ax, show_bbox=show_bbox, show_class=show_class) file_name = os.path.join(vis_dir, 'vis_{:06d}'.format(image_id)) fig.savefig(file_name, transparent=True, dpi=300) plt.close()
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 read_images(self, req): """ Reads images from a ROS service request Parameters --------- req: :obj:`ROS ServiceRequest` ROS ServiceRequest for grasp planner service """ # get the raw depth and color images as ROS Image objects raw_color = req.color_image raw_depth = req.depth_image # get the raw camera info as ROS CameraInfo object raw_camera_info = req.camera_info # wrap the camera info in a perception CameraIntrinsics object camera_intr = CameraIntrinsics( raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) # create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ### try: color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2( raw_color, "rgb8"), frame=camera_intr.frame) depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2( raw_depth, desired_encoding="passthrough"), frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) # check image sizes if color_im.height != depth_im.height or \ color_im.width != depth_im.width: msg = 'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d' % ( color_im.height, color_im.width, depth_im.height, depth_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) if color_im.height < self.min_height or color_im.width < self.min_width: msg = 'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d' % ( self.min_height, self.min_width, color_im.height, color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) return color_im, depth_im, camera_intr
def _read_depth_image(self): """ Reads a depth image from the device """ # read raw uint16 buffer im_arr = self._depth_stream.read_frame() raw_buf = im_arr.get_buffer_as_uint16() buf_array = np.array([raw_buf[i] for i in range(PrimesenseSensor.DEPTH_IM_WIDTH * PrimesenseSensor.DEPTH_IM_HEIGHT)]) # convert to image in meters depth_image = buf_array.reshape(PrimesenseSensor.DEPTH_IM_HEIGHT, PrimesenseSensor.DEPTH_IM_WIDTH) depth_image = depth_image * MM_TO_METERS # convert to meters if self._flip_images: depth_image = np.flipud(depth_image) else: depth_image = np.fliplr(depth_image) return DepthImage(depth_image, frame=self._frame)
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 _plot_grasp(self, datapoint, image_field_name, pose_field_name, gripper_mode): """ Plots a single grasp represented as a datapoint. """ image = DepthImage(datapoint[image_field_name][:, :, 0]) depth = datapoint[pose_field_name][2] width = 0 if gripper_mode == GripperMode.PARALLEL_JAW or \ gripper_mode == GripperMode.LEGACY_PARALLEL_JAW: grasp = Grasp2D(center=image.center, angle=0, depth=depth, width=0.0) width = datapoint[pose_field_name][-1] else: grasp = SuctionPoint2D(center=image.center, axis=[1, 0, 0], depth=depth) vis2d.imshow(image) vis2d.grasp(grasp, width=width)
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 run_gqcnn(depth,seg_mask): best_angle = 0; best_point = [0,0]; best_dist = 0; depth_im =DepthImage(depth.astype("float32"), frame=camera_intr.frame) color_im = ColorImage(np.zeros([imageWidth, imageHeight,3]).astype(np.uint8), frame=camera_intr.frame) print(seg_mask) segmask = BinaryImage(seg_mask) print(segmask) rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state_gqcnn = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) policy_start = time.time() try: action = policy(state_gqcnn) logger.info("Planning took %.3f sec" % (time.time() - policy_start)) best_point = [action.grasp.center[0],action.grasp.center[1]]; best_point = [action.grasp.center[0],action.grasp.center[1]]; best_angle = float(action.grasp.angle)*180/3.141592 except Exception as inst: print(inst) return best_angle,best_point,best_dist
def run_gqcnn(depth, seg_mask): best_angle = 0 best_point = [0, 0] best_dist = 0 depth_im = DepthImage(depth.astype("float32"), frame=camera_intr.frame) color_im = ColorImage(np.zeros([imageWidth, imageHeight, 3]).astype(np.uint8), frame=camera_intr.frame) segmask = BinaryImage(seg_mask) rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state_gqcnn = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) policy_start = time.time() q_value = -1 try: action = policy(state_gqcnn) best_point = [action.grasp.center[0], action.grasp.center[1]] best_angle = float(action.grasp.angle) * 180 / 3.141592 q_value = action.q_value print("inner : ", action.q_value) except Exception as inst: print(inst) return q_value
def visualize_predictions(run_dir, test_config, pred_mask_dir, pred_info_dir, show_bbox=True, show_class=True): """Visualizes predictions.""" # Create subdirectory for prediction visualizations vis_dir = os.path.join(run_dir, 'vis') depth_dir = os.path.join(test_config['path'], test_config['images']) if not os.path.exists(vis_dir): os.makedirs(vis_dir) indices_arr = np.load(os.path.join(test_config['path'], test_config['indices'])) image_ids = np.arange(indices_arr.size) ################################################################## # Process each image ################################################################## print('VISUALIZING PREDICTIONS') for image_id in tqdm(image_ids): base_name = 'image_{:06d}'.format(indices_arr[image_id]) depth_image_fn = base_name + '.npy' # Load image and ground truth data and resize for net depth_data = np.load(os.path.join(depth_dir, depth_image_fn)) image = DepthImage(depth_data).to_color().data # load mask and info r = np.load(os.path.join(pred_info_dir, 'image_{:06}.npy'.format(image_id))).item() r_masks = np.load(os.path.join(pred_mask_dir, 'image_{:06}.npy'.format(image_id))) # Must transpose from (n, h, w) to (h, w, n) if r_masks.any(): r['masks'] = np.transpose(r_masks, (1, 2, 0)) else: r['masks'] = r_masks # Visualize visualize.display_instances(image, r['rois'], r['masks'], r['class_ids'], ['bg', 'obj'], show_bbox=show_bbox, show_class=show_class) file_name = os.path.join(vis_dir, 'vis_{:06d}'.format(image_id)) plt.savefig(file_name, bbox_inches='tight', pad_inches=0) plt.close()
policy_config = config["policy"] # Make relative paths absolute. if "gqcnn_model" in policy_config["metric"]: policy_config["metric"]["gqcnn_model"] = model_path if not os.path.isabs(policy_config["metric"]["gqcnn_model"]): policy_config["metric"]["gqcnn_model"] = os.path.join( os.path.dirname(os.path.realpath(__file__)), "..", policy_config["metric"]["gqcnn_model"]) # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) # Read images. depth_data = np.load(depth_im_filename) depth_im = DepthImage(depth_data, frame=camera_intr.frame) color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), frame=camera_intr.frame) # Optionally read a segmask. segmask = None if segmask_filename is not None: segmask = BinaryImage.open(segmask_filename) valid_px_mask = depth_im.invalid_pixel_mask().inverse() if segmask is None: segmask = valid_px_mask else: segmask = segmask.mask_binary(valid_px_mask) # Inpaint.