object_blobs = filter(lambda blob: numpy.count_nonzero(blob[clean_mask]) >= 1000, object_blobs)
# take only the largest blobs
if object_blobs:
blob_mask = reduce(numpy.bitwise_or, object_blobs)
mask = object_mask & blob_mask
else:
mask = numpy.zeros_like(object_mask)
object_cloud = clean_cloud_aligned[mask[clean_mask]]
xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
object_cloud_color = map(
lambda x: x[0] + [x[1]], zip(object_cloud.tolist(), uvtexture(color, depth_uv)[mask].tolist())
)
try:
pcd.write(object_cloud_color, os.path.join(base_path, "{}_{}_segmented.pcd".format(obj, n)))
except IndexError:
print "error writing PCD"
pass
numpy.save(os.path.join(base_path, "{}_{}_mask.npy".format(obj, n)), mask)
imsave(os.path.join(base_path, "{}_{}_mask.png".format(obj, n)), mask)
imsave(os.path.join(base_path, "{}_{}_color.png".format(obj, n)), color)
color2 = uvtexture(color, depth_uv)
color2[~mask] = [0, 0, 0]
imsave(os.path.join(base_path, "{}_{}_segmented.png".format(obj, n)), color2)
print "segmented"
# object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(10*(object_cloud - object_cloud.mean(axis=0)).tolist(), uvtexture(color, depth_uv)[mask].tolist()))
# debug_cloud([ object_cloud_color ], [ xform ])
print "done"
bin_bounds = json.load(open(SHELF_DIMS_PATH))[BIN_NAME]
# for (i, p) in enumerate(camera_cloud_color):
# p[:3] = camera_cloud[i]
# debug_cloud([ shelf_cloud, camera_cloud ])
world = WorldModel()
shelf = world.loadRigidObject('klampt_models/north_shelf/shelf.obj')
shelf.setTransform(list(shelf_xform[0].flat), shelf_xform[1])
_, camera_cloud_aligned, object_cloud, mask = shelf_subtract(shelf_cloud, camera_cloud, shelf, bin_bounds)
# debug_cloud([ shelf_cloud, camera_cloud, camera_cloud_aligned ])
# debug_cloud([ object_cloud ], world=world)
debug_cloud([ shelf_cloud, camera_cloud, camera_cloud_aligned ])
debug_cloud([ object_cloud, camera_cloud_aligned ])
object_cloud_color = []
for i in range(len(mask)):
if mask[i]:
object_cloud_color.append(camera_cloud_color[i])
for (i, p) in enumerate(object_cloud_color):
p[:3] = object_cloud[i]
debug_cloud([ object_cloud_color ], world=world)
pcd.write(object_cloud_color, '/tmp/object.pcd')
_, model_cloud_aligned = fit_object_model(model_cloud, object_cloud)
debug_cloud([ model_cloud, object_cloud, model_cloud_aligned ])
def localizeSpecificObject(self, bin, object):
SHELF_DIMS_PATH = os.path.join('kb', 'shelf_dims.json')
SHELF_CLOUD_PATH = os.path.join('perception', 'segmentation', 'models', 'shelf_thin_10000.npy')
# acquire the camera image
address = { 'left': '192.168.0.103', 'right': '192.168.0.104' }[self.knowledge_base.active_limb]
#address = { 'left': '192.168.0.103', 'right': '192.168.0.103' }[self.knowledge_base.active_limb]
camera = RemoteCamera(address, xform=self.camera_xform)
camera.read()
camera.close()
# load the shelf subtraction data
bin_bounds = json.load(open(SHELF_DIMS_PATH))[bin]
# load and transform the shelf point cloud
shelf_cloud = numpy.load(open(SHELF_CLOUD_PATH))
shelf_cloud = shelf_cloud.dot(numpy.array(self.knowledge_base.shelf_xform[0]).reshape((3,3))) + self.knowledge_base.shelf_xform[1]
# load and transform the shelf model
world = WorldModel()
shelf = world.loadRigidObject(os.path.join('klampt_models', 'north_shelf', 'shelf_with_bins.obj'))
shelf.setTransform(*self.knowledge_base.shelf_xform)
# clean up the point cloud
(clean_mask, clean_cloud) = camera.clean()
_, clean_cloud_aligned, _, clean_object_mask = shelf_subtract(shelf_cloud, clean_cloud, shelf, bin_bounds, downsample=1000)
object_mask = numpy.zeros(camera.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(), camera.colorize()[clean_mask][clean_object_mask].tolist()))
#debug_cloud([ clean_cloud_aligned_color ], [ self.base_xform, self.camera_xform ])
#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'))
(target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object)
bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else []
# perform special handling
if target_object == 'rollodex_mesh_collection_jumbo_pencil_cup':
logger.info('running specialized rollodex cups detection')
return self._localizeRollodexCups(world, camera.color, clean_cloud_aligned, camera.depth_uv, clean_mask, object_mask)
# 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)
# imsave('/tmp/test.png', object_mask)
# label connected components
(object_labeled, label_count) = distance_label(camera.cloud, object_mask_dilated, threshold=0.01)
logger.info('found {} connected components'.format(label_count))
#KH: added to help debugging without changing order bin contents
if object not in bin_contents:
logger.warning("Warning, trying to detect object "+object+" not in bin contents, adding a temporary entry")
bin_contents = bin_contents + [object]
bin_contents_with_shelf = bin_contents + [ 'shelf' ]
if not label_count:
logger.warning('found no connected components')
mask = object_mask
bin_contents = self.knowledge_base.bin_contents[self.knowledge_base.target_bin]
confusion_row = dict(zip(bin_contents, [0]*len(bin_contents)))
else:
object_blobs = sorted([ object_labeled == (l+1) for l in range(label_count) ], key=lambda b: -numpy.count_nonzero(b))
#for (i, blob) in enumerate(object_blobs):
# logger.debug('blob {}: {} points'.format(i, numpy.count_nonzero(blob)))
# filter out blobs with fewer than 1000 points
min_point_count = 1000
object_blobs = filter(lambda blob: numpy.count_nonzero(blob[clean_mask]) >= min_point_count, object_blobs)
#debug_cloud([ clean_cloud_aligned[(object_mask & blob_mask)[clean_mask]] for blob_mask in object_blobs ], [ self.base_xform, self.camera_xform ], wait=False)
known_object_count = len(bin_contents)
if known_object_count == 0:
# return all large blobs
n = len(object_blobs)
# take only the largest n blobs
blob_mask = reduce(numpy.bitwise_or, object_blobs[:n])
mask = object_mask & blob_mask
logger.warning('no objects in bin... -> returning all blobs')
else:
# load the matching statistics
match_stats = json.load(open(os.path.join('perception', 'segmentation', 'match_stats.json')))
# load the object hue histograms
known_histograms = {}
for obj in bin_contents_with_shelf:
#array = numpy.load(os.path.join('perception', 'hue_array', '{}.npz'.format(obj)))['arr_0']
#known_histograms[obj] = numpy.histogram(array, bins=range(0,181), density=True)[0].reshape(-1,1).astype(numpy.float32)
known_histograms[obj] = numpy.load(os.path.join('perception', 'uv_hist2', '{}.npz'.format(obj)))['arr_0']
# compute the color validity mask
color_valid_mask = ~invalid_mask(camera.depth_uv)
#KH addition 5/23: test to see whether blobs should be broken up
if len(object_blobs)==1 and len(known_histograms) > 2:
logger.info("Trying KMeans segmentation to break up large blob")
blob_mask = reduce(numpy.bitwise_or, object_blobs)
mask = object_mask & blob_mask & color_valid_mask
labels,k,score = xyzrgb_segment_kmeans(camera.cloud[mask],camera.colorize()[mask],known_histograms.values())
labels = numpy.array(labels)
object_blobs = []
for i in xrange(len(known_histograms)):
blank = numpy.zeros_like(object_mask,dtype=numpy.bool)
blank[mask] = (labels==i)
object_blobs.append(blank)
# score each blob for each object in the bin
blob_scores = []
blob_accepts = []
blob_rejects = []
matched_blobs = {}
confusion_matrix = []
for (i, blob) in enumerate(object_blobs):
# compute the blob histogram
blob_uv = [ rgb2yuv(*rgb)[1:3] for rgb in camera.colorize()[object_mask & blob & color_valid_mask] ]
blob_histogram = make_uv_hist(blob_uv)
# compare the blob to each possible object
scores = dict([ (obj, 2 * numpy.minimum(blob_histogram, histogram).sum() - numpy.maximum(blob_histogram, histogram).sum()) for (obj, histogram) in known_histograms.items() ])
logger.debug('blob {}:'.format(i))
blob_scores.append(scores)
logger.debug(' scores: {}'.format([ '{}={}'.format(*x) for x in scores.items() ]))
# apply the cutoff to each score and associate with positive confidence
accepts = dict([ (obj, match_stats[obj]['ppv']) for obj in bin_contents_with_shelf if scores[obj] >= match_stats[obj]['cutoff'] ])
blob_accepts.append(accepts)
logger.debug(' accepts: {}'.format([ '{}={}'.format(*x) for x in accepts.items() ]))
# apply the cutoff to each score and associate with negative confidence
rejects = dict([ (obj, match_stats[obj]['npv']) for obj in bin_contents_with_shelf if scores[obj] < match_stats[obj]['cutoff'] ])
blob_rejects.append(rejects)
logger.debug(' rejects: {}'.format([ '{}={}'.format(*x) for x in rejects.items() ]))
# populate the confusion matrix
shelf_probability = 0.1
S = lambda o: match_stats[o]['specificity']
iS = lambda o: 1 - S(o)
total_probability = sum(map(S, accepts)) + sum(map(iS, rejects)) + shelf_probability
confusion_matrix.append([ [ iS(o), S(o) ][ o in accepts ] / total_probability for o in bin_contents ])
# resolve multiple assignments
if len(accepts) > 1:
# choose the object with the highest matched confidence
best_match_object = sorted(accepts.items(), key=lambda a: -a[1])[0][0]
# remove all other objects
for (obj, confidence) in accepts.items():
if obj != best_match_object:
del accepts[obj]
logger.warn('blob {} multiple accept ignored: {}={}'.format(i, obj, confidence))
if len(accepts) == 1:
# a single match so record it
matched_blobs.setdefault(accepts.keys()[0], []).append(i)
logger.info('blob {} assigned to {} by single match'.format(i, accepts.keys()[0]))
confusion_matrix = numpy.array(confusion_matrix)
logger.debug('confusion matrix:\n{}'.format(numpy.array(confusion_matrix)))
# assign blobs by least threshold difference until all objects are assigned (excluding shelf) or all blobs are assigned
while len([ k for k in matched_blobs.keys() if k != 'shelf']) < len(bin_contents) and len(sum(matched_blobs.values(), [])) < len(object_blobs):
best_match_objects = []
for (i, scores) in enumerate(blob_scores):
# only consider unmatched blobs
if i not in sum(matched_blobs.values(), []):
threshold_differences = [ [obj, match_stats[obj]['cutoff'] - score] for (obj, score) in scores.items() ]
best_match_objects.append([ i ] + sorted(threshold_differences, key=lambda d: d[1])[0])
# choose the least threshold difference across all blobs and objects
best_blob, best_object, _ = sorted(best_match_objects, key=lambda b: b[2])[0]
matched_blobs.setdefault(best_object, []).append(best_blob)
logger.warn('blob {} assigned to {} by least threshold difference'.format(best_blob, best_object))
# report unmatched blobs and objects (excluding shelf)
for obj in bin_contents:
if obj not in matched_blobs:
logger.warn('no blobs matched {}'.format(obj))
for i in range(len(object_blobs)):
if i not in sum(matched_blobs.values(), []):
logger.warn('blob {} is unassigned'.format(i))
for obj in bin_contents_with_shelf:
if obj in matched_blobs:
print obj
mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[obj] ], numpy.zeros_like(object_mask))
object_cloud = clean_cloud_aligned[mask[clean_mask]]
xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist()))
#debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ])
# check that the target object was found
if target_object not in matched_blobs:
logger.error('no blobs assigned to target object')
return None
# return blob for the selected object
mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[target_object] ], numpy.zeros_like(object_mask))
# return the confusion matrix rows for the selected object
confusion_rows = confusion_matrix[matched_blobs[target_object], :]
confusion_row = dict([ (obj, confusion_rows[:, i].mean()) for (i, obj) in enumerate(bin_contents) ])
logger.info('confusion row for {}: {}'.format(target_object, confusion_row))
object_cloud = clean_cloud_aligned[mask[clean_mask]]
#xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
xform = se3.identity()
object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist()))
#debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ])
# downsample the cloud to about 1000 points
ratio = int(len(object_cloud) / 1000)
if ratio > 1:
object_cloud_color = object_cloud_color[::ratio]
pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w'))
return (xform, object_cloud_color, confusion_row)
# apply the camera xform
if limb == 'left':
camera_xform = se3.mul(base_xform, KnowledgeBase.left_camera_offset_xform)
else:
camera_xform = se3.mul(base_xform, KnowledgeBase.right_camera_offset_xform)
# cloud = cloud.dot(numpy.array(camera_xform[0]).reshape((3, 3))) + camera_xform[1]
# # filter out the invalid points
# colorized = uvtexture(color, depth_uv)[mask]
# cloud = cloud[mask]
# cloud_color = zip(cloud.tolist(), colorized.tolist())
# print sum(mask.astype(numpy.int).flat)
camera = RemoteCamera(realsense_pc, xform=camera_xform)
camera.read()
camera.close()
# clean up the point cloud
(clean_mask, clean_cloud) = camera.clean()
_, clean_cloud_aligned, _, clean_object_mask = shelf_subtract(shelf_cloud, clean_cloud, shelf, bin_bounds, downsample=1000)
object_mask = numpy.zeros(camera.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(), camera.colorize()[clean_mask][clean_object_mask].tolist()))
debug_cloud([ shelf_cloud, clean_cloud, clean_cloud_aligned ])
debug_cloud([ clean_cloud_aligned_color ], [ base_xform, camera_xform ], world=world)
#debug_cloud([ cloud_color ], xforms=[ base_xform, camera_xform ], world=world)
pcd.write(clean_cloud_aligned_color, '/tmp/test.pcd')
def _localizeRollodexCups(self, world, color, clean_cloud_aligned, depth_uv, clean_mask, object_mask):
# copy the uv map because it is modified below
# also flip x/y since numpy is row/column
uv = depth_uv[:,:,::-1].copy()
# scale the uv coordinates by the source image size
for i in range(2):
uv[:,:,i] = numpy.clip(uv[:,:,i] * color.shape[i], 0, color.shape[i]-1)
# round to the nearest pixel
uv = uv.round().astype(numpy.int)
# filter the uv mapping using the bounding box coordinates
((bb_top, bb_left), (bb_bottom, bb_right)) = match_rollodex_template(color)
from scipy.misc import imsave
imsave('/tmp/cups.png', color[bb_top:bb_bottom, bb_left:bb_right])
logger.debug('rollodex cups template match bounding box: {}'.format(((bb_top, bb_left), (bb_bottom, bb_right))))
cups_mask = reduce(numpy.bitwise_and, [ uv[:,:,0] > bb_top, uv[:,:,0] < bb_bottom, uv[:,:,1] > bb_left, uv[:,:,1] < bb_right ])
#imsave('/tmp/cups_uv.png', uv)
imsave('/tmp/cups_mask.png', cups_mask)
valid_depth_pixels = numpy.count_nonzero(cups_mask)
if valid_depth_pixels == 0:
logger.error('rollodex cups template has no valid depth pixels')
return None
logger.debug('rollodex cups template has {} valid depth pixels'.format(valid_depth_pixels))
# get the point cloud corresponding to the cups bounding box
mask = object_mask & cups_mask
cups_cloud = clean_cloud_aligned[mask[clean_mask]]
cups_front = cups_cloud[:,0].min()
cups_mean = cups_cloud.mean(axis=0)
known_cups_width = 0.10 - 0.02
known_cups_height = 0.13 - 0.02
# generate a fake point cloud for the cups
n = 32
fake_cloud = numpy.dstack((
cups_front * numpy.ones((n, n)),
numpy.dstack(numpy.meshgrid(
known_cups_width * numpy.linspace(-0.5, 0.5, n) + cups_mean[1],
known_cups_height * numpy.linspace(-0.5, 0.5, n) + cups_mean[2],
indexing='ij'
))
))
xform = se3.identity()
object_cloud = fake_cloud.reshape((-1, 3))
(target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object)
bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else []
confusion_row = dict([ (obj, [0, 1][obj == 'rollodex_mesh_collection_jumbo_pencil_cup']) for obj in bin_contents ])
logger.info('confusion row for {}: {}'.format(target_object, confusion_row))
object_cloud_color = [ (p + [ [0, 0, 0] ]) for p in object_cloud.tolist() ]
#debug_cloud([ object_cloud_color ], [ (so3.identity(), cups_mean) ], world=world)
pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(target_object, target_bin), 'w'))
return (xform, object_cloud_color, confusion_row)
