Exemple #1
0
# 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')
Exemple #2
0
    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)