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