def load_histogram(obj, n):
hist_path = os.path.join(base_path, "{}_{}_hist.npy".format(obj, n))
if os.path.exists(hist_path):
blob_histogram = numpy.load(hist_path)
else:
color = numpy.load(os.path.join(base_path, "{}_{}_color.npy".format(obj, n)))
depth_uv = numpy.load(os.path.join(base_path, "{}_{}_depth_uv.npy".format(obj, n)))
if obj == "shelf":
mask = numpy.ones((color.shape[0], color.shape[1]), dtype=numpy.bool)
else:
mask = numpy.load(os.path.join(base_path, "{}_{}_mask.npy".format(obj, n)))
color2 = uvtexture(color, depth_uv)
color_valid_mask = ~invalid_mask(depth_uv)
blob_uv = [rgb2yuv(*rgb)[1:3] for rgb in color2[mask & color_valid_mask]]
blob_histogram = make_uv_hist(blob_uv)
numpy.save(hist_path, blob_histogram)
return blob_histogram
cloud = numpy.load(os.path.join(base_path, "{}_{}_cloud.npy".format(obj, n)))
# clean up the point cloud
clean_mask = invalid_mask(cloud)
clean_cloud = cloud[clean_mask]
_, clean_cloud_aligned, _, clean_object_mask = shelf_subtract(
shelf_cloud, clean_cloud, shelf, bin_bounds, downsample=1000
)
object_mask = numpy.zeros(cloud.shape[:2], dtype=numpy.bool)
object_mask[clean_mask] = clean_object_mask
clean_cloud_aligned_color = map(
lambda x: x[0] + [x[1]],
zip(
clean_cloud_aligned[clean_object_mask].tolist(),
uvtexture(color, depth_uv)[clean_mask][clean_object_mask].tolist(),
),
)
# debug_cloud([ clean_cloud_aligned_color ], world=world)
# debug_cloud([ shelf_cloud, clean_cloud, clean_cloud_aligned ])
# pcd.write(clean_cloud_aligned_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w'))
# dilate the object mask
from scipy.misc import imsave
from scipy.ndimage.morphology import binary_dilation
object_mask_dilated = binary_dilation(object_mask, iterations=2)
# label connected components
from scipy.ndimage.measurements import label
for obj in objects:
print 'swtich to', obj
histograms = []
for n in range(6):
print 'take', n,
color = numpy.load(os.path.join(base_path, '{}_{}_color.npy'.format(obj, n)))
depth_uv = numpy.load(os.path.join(base_path, '{}_{}_depth_uv.npy'.format(obj, n)))
if obj == 'shelf':
mask = numpy.ones((color.shape[0], color.shape[1]), dtype=numpy.bool)
else:
mask = numpy.load(os.path.join(base_path, '{}_{}_mask.npy'.format(obj, n)))
color2 = uvtexture(color, depth_uv)
color_valid_mask = ~invalid_mask(depth_uv)
blob_uv = [ rgb2yuv(*rgb)[1:3] for rgb in color2[mask & color_valid_mask] ]
if blob_uv:
blob_histogram = make_uv_hist(blob_uv)
histograms.append(blob_histogram)
hist_path = os.path.join(base_path, '{}_{}_hist.npy'.format(obj, n))
numpy.save(hist_path, blob_histogram)
print 'histogram'
else:
print 'skipped'
if histograms:
composite_histogram = sum(histograms) / len(histograms)