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 show(self, filename=None, dpi=100):
""" Show a figure. """
if self._logging_dir is None:
vis.show()
else:
filename = os.path.join(self._policy_dir, filename)
vis.savefig(filename, dpi=dpi)
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 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 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 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 score_plane_intersection(pc, cell_pc, cell_centroid, shadow, clutter):
"""
The score of this cell is the number of points that intersect with the plane.
Assumes that the cell_pc is the set of all points in both the plane and the cell.
The score is higher if more points intersect (since clutter and box edges are not in the flat plane, they don't intersect).
"""
length, width, height = shadow.extents
minx, maxx = cell_centroid[0] - (length / 2), cell_centroid[0] + (length /
2)
miny, maxy = cell_centroid[1] - (width / 2), cell_centroid[1] + (width / 2)
intersecting_pts = cell_pc[minx < cell_pc[:, 0]]
intersecting_pts = intersecting_pts[intersecting_pts[:, 0] < maxx]
intersecting_pts = intersecting_pts[miny < intersecting_pts[:, 1]]
intersecting_pts = intersecting_pts[intersecting_pts[:, 1] < maxy]
#clutter = clutter.data.T
clutter = clutter[minx < clutter[:, 0]]
clutter = clutter[clutter[:, 0] < maxx]
clutter = clutter[miny < clutter[:, 1]]
clutter = clutter[clutter[:, 1] < maxy]
if len(clutter) == 0:
return intersecting_pts, len(intersecting_pts)
#print("Int pts shape = " + str(intersecting_pts.shape))
#print("clutter shape = " + str(clutter.shape))
#print("Clutter: " + str(clutter))
clutter_pts = np.in1d(intersecting_pts, clutter)
if (len(clutter_pts) % 3 != 0):
print("Int pts shape = " + str(intersecting_pts.shape))
print("clutter shape = " + str(clutter.shape))
print("Clutter pts shape = " + str(clutter_pts.shape))
clutter_pts.shape = (len(clutter_pts) // 3, 3)
if len(clutter_pts) == 0:
return intersecting_pts, len(intersecting_pts)
clutter_pts = np.apply_along_axis(np.all, 1, clutter_pts)
int_pts_pc = PointCloud(intersecting_pts.T, pc.frame)
clutter_pc = PointCloud(clutter.T, pc.frame)
di = ci.project_to_image(int_pts_pc)
clutter_di = ci.project_to_image(clutter_pc)
bi = di.invalid_pixel_mask()
diff = bi.diff_with_target(clutter_di)
vis2d.figure()
vis2d.imshow(clutter)
vis2d.imshow(diff)
vis2d.show()
#print("Length of clutter pts = " + str(len(clutter_pts)))
return intersecting_pts, -1 * len(clutter_pts)
"""
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 _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 _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 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()
# optionally read a segmask
segmask = None
if segmask_filename is not None:
segmask = BinaryImage.open(segmask_filename)
print "SegMask Opening __________________"
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=inpaint_rescale_factor)
if True: #'input_images' in policy_config['vis'].keys() and policy_config['vis']['input_images']:
vis.figure(size=(10, 10))
num_plot = 1
if segmask is not None:
num_plot = 2
vis.subplot(1, num_plot, 1)
vis.imshow(depth_im)
if segmask is not None:
vis.subplot(1, num_plot, 2)
vis.imshow(segmask)
vis.show()
flag_mask = "Y" #raw_input("Do you want to use mask? [Y/n]")
if flag_mask == "n":
print "------ YOU CHOOSE WITHOUT MASK -----"
segmask = None
# 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.
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
if "input_images" in policy_config["vis"] and policy_config["vis"][
"input_images"]:
vis.figure(size=(10, 10))
num_plot = 1
if segmask is not None:
num_plot = 2
vis.subplot(1, num_plot, 1)
vis.imshow(depth_im)
if segmask is not None:
vis.subplot(1, num_plot, 2)
vis.imshow(segmask)
vis.show()
# Create state.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Set input sizes for fully-convolutional policy.
# read config
config = YamlConfig(config_filename)
policy_config = config['policy']
# 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,
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 _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 _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))
def compute_object_pose(self):
camera_intrinsics = self.sensor.ir_intrinsics
# get the images from the sensor
color_image, depth_image, _ = self.sensor.frames()
# inpaint to remove holes
inpainted_color_image = color_image.inpaint(
rescale_factor=self.config['inpaint_rescale_factor'])
inpainted_depth_image = depth_image.inpaint(
rescale_factor=self.config['inpaint_rescale_factor'])
detector_cfg = self.config['detector']
detector_cfg['image_width'] = inpainted_depth_image.width // 3
detector_cfg['image_height'] = inpainted_depth_image.height // 3
detector = RgbdDetectorFactory.detector('point_cloud_box')
detection = detector.detect(inpainted_color_image,
inpainted_depth_image,
detector_cfg,
camera_intrinsics,
self.tf_camera_world,
vis_foreground=False,
vis_segmentation=False)[0]
if self.config['vis']['vis_detector_output']:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(detection.color_thumbnail)
vis.subplot(1, 2, 2)
vis.imshow(detection.depth_thumbnail)
vis.show()
boundingBox = BoundingBox()
boundingBox.minY = detection.bounding_box.min_pt[0]
boundingBox.minX = detection.bounding_box.min_pt[1]
boundingBox.maxY = detection.bounding_box.max_pt[0]
boundingBox.maxX = detection.bounding_box.max_pt[1]
try:
start_time = time.time()
planned_grasp_data = self.plan_grasp_client(
inpainted_color_image.rosmsg, inpainted_depth_image.rosmsg,
camera_intrinsics.rosmsg, boundingBox, None)
grasp_plan_time = time.time() - start_time
rospy.loginfo("Planning time: {}".format(grasp_plan_time))
rospy.loginfo("Camera CS planned_grasp_data:\n{}".format(
planned_grasp_data.grasp.pose))
if planned_grasp_data.grasp.q_value < 0.20:
rospy.loginfo("Q value is too small : {}".format(
planned_grasp_data.grasp.q_value))
return None
grasp_camera_pose = planned_grasp_data.grasp
rospy.loginfo('Processing Grasp')
self.broadcast_pose_as_transform(self.tf_camera_world.from_frame,
"object_link",
grasp_camera_pose.pose)
# create Stamped ROS Transform
camera_world_transform = TransformStamped()
camera_world_transform.header.stamp = rospy.Time.now()
camera_world_transform.header.frame_id = self.tf_camera_world.from_frame
camera_world_transform.child_frame_id = self.tf_camera_world.to_frame
camera_world_transform.transform.translation.x = self.tf_camera_world.translation[
0]
camera_world_transform.transform.translation.y = self.tf_camera_world.translation[
1]
camera_world_transform.transform.translation.z = self.tf_camera_world.translation[
2]
q = self.tf_camera_world.quaternion
camera_world_transform.transform.rotation.x = q[1]
camera_world_transform.transform.rotation.y = q[2]
camera_world_transform.transform.rotation.z = q[3]
camera_world_transform.transform.rotation.w = q[0]
grasp_world_pose = tf2_geometry_msgs.do_transform_pose(
grasp_camera_pose, camera_world_transform)
# if grasp_world_pose.pose.position.z < 0.05:
# rospy.logwarn("Predicted pose Z value is less than 5 cm -> bad pose")
# return None
# Hack z position
# grasp_world_pose.pose.position.z += 0.0075
# Hack orientation to 90 degree vertical pose
grasp_world_pose.pose.orientation.x = -0.718339806303
grasp_world_pose.pose.orientation.y = 0.00601026421019
grasp_world_pose.pose.orientation.z = 0.695637686512
grasp_world_pose.pose.orientation.w = 0.00632522789696
rospy.loginfo(
"World CS planned_grasp_data:\n{}".format(grasp_world_pose))
return grasp_world_pose.pose
except rospy.ServiceException as e:
rospy.logerr("Service call failed: \n %s" % e)
return None
def _plan_grasp(self,
color_im,
depth_im,
camera_intr,
bounding_box=None,
segmask=None):
"""Grasp planner request handler.
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS `ServiceRequest` for grasp planner service.
"""
rospy.loginfo("Planning Grasp")
# Inpaint images.
color_im = color_im.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
depth_im = depth_im.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
# Init segmask.
if segmask is None:
segmask = BinaryImage(255 *
np.ones(depth_im.shape).astype(np.uint8),
frame=color_im.frame)
# Visualize.
if self.cfg["vis"]["color_image"]:
vis.imshow(color_im)
vis.show()
if self.cfg["vis"]["depth_image"]:
vis.imshow(depth_im)
vis.show()
if self.cfg["vis"]["segmask"] and segmask is not None:
vis.imshow(segmask)
vis.show()
# Aggregate color and depth images into a single
# BerkeleyAutomation/perception `RgbdImage`.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# Mask bounding box.
if bounding_box is not None:
# Calc bb parameters.
min_x = bounding_box.minX
min_y = bounding_box.minY
max_x = bounding_box.maxX
max_y = bounding_box.maxY
# Contain box to image->don't let it exceed image height/width
# bounds.
if min_x < 0:
min_x = 0
if min_y < 0:
min_y = 0
if max_x > rgbd_im.width:
max_x = rgbd_im.width
if max_y > rgbd_im.height:
max_y = rgbd_im.height
# Mask.
bb_segmask_arr = np.zeros([rgbd_im.height, rgbd_im.width])
bb_segmask_arr[min_y:max_y, min_x:max_x] = 255
bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8),
segmask.frame)
segmask = segmask.mask_binary(bb_segmask)
# Visualize.
if self.cfg["vis"]["rgbd_state"]:
masked_rgbd_im = rgbd_im.mask_binary(segmask)
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(masked_rgbd_im.color)
vis.subplot(1, 2, 2)
vis.imshow(masked_rgbd_im.depth)
vis.show()
# Create an `RgbdImageState` with the cropped `RgbdImage` and
# `CameraIntrinsics`.
rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Execute policy.
try:
return self.execute_policy(rgbd_state, self.grasping_policy,
self.grasp_pose_publisher,
camera_intr.frame)
except NoValidGraspsException:
rospy.logerr(
("While executing policy found no valid grasps from sampled"
" antipodal point pairs. Aborting Policy!"))
raise rospy.ServiceException(
("While executing policy found no valid grasps from sampled"
" antipodal point pairs. Aborting Policy!"))
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
depth_im = depth_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
rgbd_state = RgbdImageState(rgbd_im, camera_int)
#%%
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)
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 _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal.
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:`SuctionPoint2D`
List of 2D suction point candidates.
"""
# Compute edge pixels.
filter_start = time()
if self._depth_gaussian_sigma > 0:
depth_im_mask = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_gaussian_sigma)
else:
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
self._logger.debug("Filtering took %.3f sec" % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# Project to get the point cloud.
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
self._logger.debug("Normal cloud took %.3f sec" %
(time() - cloud_start))
# Randomly sample points and add to image.
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# Sample a point uniformly at random.
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
# depth = point_cloud_im[center.y, center.x][2]
orientation = 2 * np.pi * np.random.rand()
# Update number of tries.
k += 1
# Skip bad axes.
if np.linalg.norm(axis) == 0:
continue
# Rotation matrix.
x_axis = axis
y_axis = np.array([axis[1], -axis[0], 0])
if np.linalg.norm(y_axis) == 0:
y_axis = np.array([1, 0, 0])
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.cross(x_axis, y_axis)
R = np.array([x_axis, y_axis, z_axis]).T
# R_orig = np.copy(R)
R = R.dot(RigidTransform.x_axis_rotation(orientation))
t = point_cloud_im[center.y, center.x]
pose = RigidTransform(rotation=R,
translation=t,
from_frame="grasp",
to_frame=camera_intr.frame)
# Check center px dist from boundary.
if (center_px[0] < self._min_dist_from_boundary
or center_px[1] < self._min_dist_from_boundary
or center_px[1] >
depth_im.height - self._min_dist_from_boundary
or center_px[0] >
depth_im.width - self._min_dist_from_boundary):
continue
# Keep if the angle between the camera optical axis and the suction
# direction is less than a threshold.
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# Check distance to ensure sample diversity.
candidate = MultiSuctionPoint2D(pose, camera_intr=camera_intr)
# Check constraint satisfaction.
if constraint_fn is None or constraint_fn(candidate):
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return suction_points
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 _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal.
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).
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
List of 2D suction point candidates.
"""
# Compute edge pixels.
filter_start = time()
if self._depth_gaussian_sigma > 0:
depth_im_mask = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_gaussian_sigma)
else:
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
self._logger.debug("Filtering took %.3f sec" % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# Project to get the point cloud.
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
self._logger.debug("Normal cloud took %.3f sec" %
(time() - cloud_start))
# Randomly sample points and add to image.
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# Sample a point uniformly at random.
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
depth = point_cloud_im[center.y, center.x][2]
# Update number of tries.
k += 1
# Check center px dist from boundary.
if (center_px[0] < self._min_dist_from_boundary
or center_px[1] < self._min_dist_from_boundary
or center_px[1] >
depth_im.height - self._min_dist_from_boundary
or center_px[0] >
depth_im.width - self._min_dist_from_boundary):
continue
# Perturb depth.
delta_depth = self._depth_rv.rvs(size=1)[0]
depth = depth + delta_depth
# Keep if the angle between the camera optical axis and the suction
# direction is less than a threshold.
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# Create candidate grasp.
candidate = SuctionPoint2D(center,
axis,
depth,
camera_intr=camera_intr)
# Check constraint satisfaction.
if constraint_fn is None or constraint_fn(candidate):
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return suction_points
interp='nearest')
# 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()
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()
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()
