def load(save_dir):
if not os.path.exists(save_dir):
raise ValueError('Directory %s does not exist!' % (save_dir))
color_image_filename = os.path.join(save_dir, 'color.png')
depth_image_filename = os.path.join(save_dir, 'depth.npy')
camera_intr_filename = os.path.join(save_dir, 'camera.intr')
segmask_filename = os.path.join(save_dir, 'segmask.npy')
obj_segmask_filename = os.path.join(save_dir, 'obj_segmask.npy')
state_filename = os.path.join(save_dir, 'state.pkl')
camera_intr = CameraIntrinsics.load(camera_intr_filename)
color = ColorImage.open(color_image_filename, frame=camera_intr.frame)
depth = DepthImage.open(depth_image_filename, frame=camera_intr.frame)
segmask = None
if os.path.exists(segmask_filename):
segmask = BinaryImage.open(segmask_filename,
frame=camera_intr.frame)
obj_segmask = None
if os.path.exists(obj_segmask_filename):
obj_segmask = SegmentationImage.open(obj_segmask_filename,
frame=camera_intr.frame)
fully_observed = None
if os.path.exists(state_filename):
fully_observed = pkl.load(open(state_filename, 'rb'))
return RgbdImageState(RgbdImage.from_color_and_depth(color, depth),
camera_intr,
segmask=segmask,
obj_segmask=obj_segmask,
fully_observed=fully_observed)
policy_config["metric"]["gqcnn_model"])
# Setup sensor.
camera_intr = CameraIntrinsics.load(camera_intr_filename)
# Read images.
depth_data = np.load(depth_im_filename)
depth_im = DepthImage(depth_data, frame=camera_intr.frame)
color_im = ColorImage(np.zeros([depth_im.height, depth_im.width,
3]).astype(np.uint8),
frame=camera_intr.frame)
# Optionally read a segmask.
segmask = None
if segmask_filename is not None:
segmask = BinaryImage.open(segmask_filename)
valid_px_mask = depth_im.invalid_pixel_mask().inverse()
if segmask is None:
segmask = valid_px_mask
else:
segmask = segmask.mask_binary(valid_px_mask)
# Inpaint.
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
if "input_images" in policy_config["vis"] and policy_config["vis"][
"input_images"]:
vis.figure(size=(10, 10))
num_plot = 1
if segmask is not None:
num_plot = 2
plan_grasp_segmask = rospy.ServiceProxy(
'%s/grasp_planner_segmask' % (namespace), GQCNNGraspPlannerSegmask)
cv_bridge = CvBridge()
# setup sensor
camera_intr = CameraIntrinsics.load(camera_intr_filename)
# read images
depth_im = DepthImage.open(depth_im_filename, frame=camera_intr.frame)
color_im = ColorImage(np.zeros([depth_im.height, depth_im.width,
3]).astype(np.uint8),
frame=camera_intr.frame)
# read segmask
if segmask_filename is not None:
segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame)
grasp_resp = plan_grasp_segmask(color_im.rosmsg, depth_im.rosmsg,
camera_intr.rosmsg, segmask.rosmsg)
else:
grasp_resp = plan_grasp(color_im.rosmsg, depth_im.rosmsg,
camera_intr.rosmsg)
grasp = grasp_resp.grasp
# convert to a grasp action
grasp_type = grasp.grasp_type
if grasp_type == GQCNNGrasp.PARALLEL_JAW:
center = Point(np.array([grasp.center_px[0], grasp.center_px[1]]),
frame=camera_intr.frame)
grasp_2d = Grasp2D(center,
grasp.angle,
grasp.depth,
def detect(detector_type, config, run_dir, test_config):
"""Run PCL-based detection on a depth-image-based dataset.
Parameters
----------
config : dict
config for a PCL detector
run_dir : str
Directory to save outputs in. Output will be saved in pred_masks, pred_info,
and modal_segmasks_processed subdirectories.
test_config : dict
config containing dataset information
"""
##################################################################
# Set up output directories
##################################################################
# Create subdirectory for prediction masks
pred_dir = os.path.join(run_dir, 'pred_masks')
mkdir_if_missing(pred_dir)
# Create subdirectory for prediction scores & bboxes
pred_info_dir = os.path.join(run_dir, 'pred_info')
mkdir_if_missing(pred_info_dir)
# Create subdirectory for transformed GT segmasks
resized_segmask_dir = os.path.join(run_dir, 'modal_segmasks_processed')
mkdir_if_missing(resized_segmask_dir)
##################################################################
# Set up input directories
##################################################################
dataset_dir = test_config['path']
indices_arr = np.load(os.path.join(dataset_dir, test_config['indices']))
# Input depth image data (numpy files, not .pngs)
depth_dir = os.path.join(dataset_dir, test_config['images'])
# Input GT binary masks dir
gt_mask_dir = os.path.join(dataset_dir, test_config['masks'])
# Input binary mask data
if 'bin_masks' in test_config.keys():
bin_mask_dir = os.path.join(dataset_dir, test_config['bin_masks'])
# Input camera intrinsics
camera_intrinsics_fn = os.path.join(dataset_dir, 'camera_intrinsics.intr')
camera_intrs = CameraIntrinsics.load(camera_intrinsics_fn)
image_ids = np.arange(indices_arr.size)
##################################################################
# Process each image
##################################################################
for image_id in tqdm(image_ids):
base_name = 'image_{:06d}'.format(indices_arr[image_id])
output_name = 'image_{:06d}'.format(image_id)
depth_image_fn = base_name + '.npy'
# Extract depth image
depth_data = np.load(os.path.join(depth_dir, depth_image_fn))
depth_im = DepthImage(depth_data, camera_intrs.frame)
depth_im = depth_im.inpaint(0.25)
# Mask out bin pixels if appropriate/necessary
if bin_mask_dir:
mask_im = BinaryImage.open(
os.path.join(bin_mask_dir, base_name + '.png'),
camera_intrs.frame)
mask_im = mask_im.resize(depth_im.shape[:2])
depth_im = depth_im.mask_binary(mask_im)
point_cloud = camera_intrs.deproject(depth_im)
point_cloud.remove_zero_points()
pcl_cloud = pcl.PointCloud(point_cloud.data.T.astype(np.float32))
tree = pcl_cloud.make_kdtree()
if detector_type == 'euclidean':
segmentor = pcl_cloud.make_EuclideanClusterExtraction()
segmentor.set_ClusterTolerance(config['tolerance'])
elif detector_type == 'region_growing':
segmentor = pcl_cloud.make_RegionGrowing(ksearch=50)
segmentor.set_NumberOfNeighbours(config['n_neighbors'])
segmentor.set_CurvatureThreshold(config['curvature'])
segmentor.set_SmoothnessThreshold(config['smoothness'])
else:
print('PCL detector type not supported')
exit()
segmentor.set_MinClusterSize(config['min_cluster_size'])
segmentor.set_MaxClusterSize(config['max_cluster_size'])
segmentor.set_SearchMethod(tree)
cluster_indices = segmentor.Extract()
# Set up predicted masks and metadata
indiv_pred_masks = []
r_info = {
'rois': [],
'scores': [],
'class_ids': [],
}
for i, cluster in enumerate(cluster_indices):
points = pcl_cloud.to_array()[cluster]
indiv_pred_mask = camera_intrs.project_to_image(
PointCloud(points.T, frame=camera_intrs.frame)).to_binary()
indiv_pred_mask.data[indiv_pred_mask.data > 0] = 1
indiv_pred_masks.append(indiv_pred_mask.data)
# Compute bounding box, score, class_id
nonzero_pix = np.nonzero(indiv_pred_mask.data)
min_x, max_x = np.min(nonzero_pix[1]), np.max(nonzero_pix[1])
min_y, max_y = np.min(nonzero_pix[0]), np.max(nonzero_pix[0])
r_info['rois'].append([min_y, min_x, max_y, max_x])
r_info['scores'].append(1.0)
r_info['class_ids'].append(1)
r_info['rois'] = np.array(r_info['rois'])
r_info['scores'] = np.array(r_info['scores'])
r_info['class_ids'] = np.array(r_info['class_ids'])
# Write the predicted masks and metadata
if indiv_pred_masks:
pred_mask_output = np.stack(indiv_pred_masks).astype(np.uint8)
else:
pred_mask_output = np.array(indiv_pred_masks).astype(np.uint8)
# Save out ground-truth mask as array of shape (n, h, w)
indiv_gt_masks = []
gt_mask = cv2.imread(os.path.join(gt_mask_dir, base_name + '.png'))
gt_mask = cv2.resize(gt_mask,
(depth_im.shape[1], depth_im.shape[0])).astype(
np.uint8)[:, :, 0]
num_gt_masks = np.max(gt_mask)
for i in range(1, num_gt_masks + 1):
indiv_gt_mask = (gt_mask == i)
if np.any(indiv_gt_mask):
indiv_gt_masks.append(gt_mask == i)
gt_mask_output = np.stack(indiv_gt_masks)
np.save(os.path.join(resized_segmask_dir, output_name + '.npy'),
gt_mask_output)
np.save(os.path.join(pred_dir, output_name + '.npy'), pred_mask_output)
np.save(os.path.join(pred_info_dir, output_name + '.npy'), r_info)
print('Saved prediction masks to:\t {}'.format(pred_dir))
print('Saved prediction info (bboxes, scores, classes) to:\t {}'.format(
pred_info_dir))
print('Saved transformed GT segmasks to:\t {}'.format(resized_segmask_dir))
return pred_dir, pred_info_dir, resized_segmask_dir
def create_dataset(self, datapoints_per_file=100):
""" Compress all known data into a numpy dataset """
# create compressed data dir
if not os.path.exists(self.compressed_data_path):
os.mkdir(self.compressed_data_path)
# write all data
file_num = 0
num_recorded = 0
data_tensors = {}
for row in self._data_csv:
# add all attributes of the current row to the tensor
if not row['completed']:
continue
for name, value in row.iteritems():
# read raw data
data = None
if name == 'input_depth_im':
if os.path.exists(value):
data = DepthImage.open(value).raw_data[np.newaxis, ...]
elif name == 'input_binary_im':
if os.path.exists(value):
data = BinaryImage.open(value).raw_data[np.newaxis,
...]
elif name == 'input_pose':
if os.path.exists(value):
data = np.load(value)[np.newaxis, ...]
elif name == 'lifted_object' or name == 'human_label' \
or name == 'gripper_width' or name == 'found_grasp' \
or name == 'gripper_torque':
data = float(value)
if data is None:
continue
# update tensor
if name not in data_tensors or data_tensors[name] is None:
data_tensors[name] = data
else:
data_tensors[name] = np.r_[data_tensors[name], data]
num_recorded += 1
# write to file if necessary
if num_recorded >= datapoints_per_file:
# save each tensor
for name, tensor in data_tensors.iteritems():
output_name = self.experiment_data_output_names[name]
if output_name is None:
output_name = name
filename = os.path.join(
self.compressed_data_path,
'%s_%05d.npz' % (output_name, file_num))
np.savez_compressed(filename, tensor)
# update for next round
for name in data_tensors.keys():
del data_tensors[name]
data_tensors[name] = None
file_num += 1
num_recorded = 0
# save final tensor
if num_recorded > 0:
for name, tensor in data_tensors.iteritems():
output_name = self.experiment_data_output_names[name]
if output_name is None:
output_name = name
filename = os.path.join(
self.compressed_data_path,
'%s_%05d.npz' % (output_name, file_num))
np.savez_compressed(filename, tensor)
type=float,
default=1.0,
help='scale factor to apply to the mesh')
parser.add_argument('--output_filename',
type=str,
default=None,
help='output obj filename')
args = parser.parse_args()
image_filename = args.input_image
extrusion = args.extrusion
scale_factor = args.scale_factor
output_filename = args.output_filename
# read the image
binary_im = BinaryImage.open(image_filename)
sdf = binary_im.to_sdf()
#plt.figure()
#plt.imshow(sdf)
#plt.show()
# convert to a mesh
mesh = ImageToMeshConverter.binary_image_to_mesh(binary_im,
extrusion=extrusion,
scale_factor=scale_factor)
vis.figure()
vis.mesh(mesh)
vis.show()
# optionally save
if output_filename is not None:
def detect(detector_type, config, run_dir, test_config):
"""Run RGB Object Proposal-based detection on a color-image-based dataset.
Parameters
----------
detector_type : str
type of detector (either mcg or gop)
config : dict
config for a GOP/MCG detector
run_dir : str
Directory to save outputs in. Output will be saved in pred_masks, pr$
and modal_segmasks_processed subdirectories.
test_config : dict
config containing dataset information
"""
##################################################################
# Set up output directories
##################################################################
# Create subdirectory for prediction masks
pred_dir = os.path.join(run_dir, 'pred_masks')
mkdir_if_missing(pred_dir)
# Create subdirectory for prediction scores & bboxes
pred_info_dir = os.path.join(run_dir, 'pred_info')
mkdir_if_missing(pred_info_dir)
# Create subdirectory for transformed GT segmasks
resized_segmask_dir = os.path.join(run_dir, 'modal_segmasks_processed')
mkdir_if_missing(resized_segmask_dir)
##################################################################
# Set up input directories
##################################################################
dataset_dir = test_config['path']
indices_arr = np.load(os.path.join(dataset_dir, test_config['indices']))
# Input depth image data (numpy files, not .pngs)
rgb_dir = os.path.join(dataset_dir, test_config['images'])
# Input GT binary masks dir
gt_mask_dir = os.path.join(dataset_dir, test_config['masks'])
# Input binary mask data
if 'bin_masks' in list(test_config.keys()):
bin_mask_dir = os.path.join(dataset_dir, test_config['bin_masks'])
image_ids = np.arange(indices_arr.size)
##################################################################
# Process each image
##################################################################
for image_id in tqdm(image_ids):
base_name = 'image_{:06d}'.format(indices_arr[image_id])
output_name = 'image_{:06d}'.format(image_id)
rgb_image_fn = os.path.join(rgb_dir, base_name + '.png')
# Run GOP detector
if detector_type == 'gop':
detector = GOP()
elif detector_type == 'mcg':
mcg_dir = os.path.join(dataset_dir, 'mcg', config['mode'])
detector = MCG(mcg_dir, nms_thresh=config['nms_thresh'])
pred_mask = detector.detect(rgb_image_fn)
# Save out ground-truth mask as array of shape (n, h, w)
indiv_gt_masks = []
gt_mask = cv2.imread(os.path.join(gt_mask_dir, base_name +
'.png')).astype(np.uint8)[:, :, 0]
num_gt_masks = np.max(gt_mask)
for i in range(1, num_gt_masks + 1):
indiv_gt_masks.append(gt_mask == i)
gt_mask_output = np.stack(indiv_gt_masks)
np.save(os.path.join(resized_segmask_dir, output_name + '.npy'),
gt_mask_output)
# Set up predicted masks and metadata
indiv_pred_masks = []
r_info = {
'rois': [],
'scores': [],
'class_ids': [],
}
if bin_mask_dir:
mask_im = BinaryImage.open(
os.path.join(bin_mask_dir, base_name + '.png'), 'phoxi')
bin_mask = cv2.resize(mask_im.data,
(pred_mask.shape[1], pred_mask.shape[0]))
# Number of predictions to use (larger number means longer time)
num_pred_masks = min(pred_mask.shape[2], 100)
# num_pred_masks = pred_mask.shape[2]
for i in range(1, num_pred_masks + 1):
# Extract individual mask
indiv_pred_mask = pred_mask[:, :, i - 1]
if not np.any(indiv_pred_mask):
continue
if bin_mask_dir:
inter = np.logical_and(bin_mask, indiv_pred_mask)
frac_overlap = np.sum(inter) / np.sum(indiv_pred_mask)
if frac_overlap <= 0.5:
continue
inter = np.logical_and(indiv_pred_mask,
np.sum(indiv_pred_masks, axis=0))
frac_overlap = np.sum(inter) / np.sum(indiv_pred_mask)
if frac_overlap >= 0.5:
continue
indiv_pred_masks.append(indiv_pred_mask)
# Compute bounding box, score, class_id
nonzero_pix = np.nonzero(indiv_pred_mask)
min_x, max_x = np.min(nonzero_pix[1]), np.max(nonzero_pix[1])
min_y, max_y = np.min(nonzero_pix[0]), np.max(nonzero_pix[0])
r_info['rois'].append([min_y, min_x, max_y, max_x])
if detector.mock_score:
# Generates a meaningful mock score for MCG (first region scores
# highest, etc.)
r_info['scores'].append(-i)
else:
r_info['scores'].append(1.0)
r_info['class_ids'].append(1)
r_info['rois'] = np.array(r_info['rois'])
r_info['scores'] = np.array(r_info['scores'])
r_info['class_ids'] = np.array(r_info['class_ids'])
# Write the predicted masks and metadata
pred_mask_output = np.stack(indiv_pred_masks).astype(
np.uint8) if indiv_pred_masks else np.array([])
np.save(os.path.join(pred_dir, output_name + '.npy'), pred_mask_output)
np.save(os.path.join(pred_info_dir, output_name + '.npy'), r_info)
pred_mask_output = np.stack(indiv_pred_masks).astype(np.uint8)
print(('Saved prediction masks to:\t {}'.format(pred_dir)))
print(('Saved prediction info (bboxes, scores, classes) to:\t {}'.format(
pred_info_dir)))
print(
('Saved transformed GT segmasks to:\t {}'.format(resized_segmask_dir)))
return pred_dir, pred_info_dir, resized_segmask_dir
