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.
