Beispiel #1
0
    def localizeSpecificObject_old(self, bin, object):
        (xform, cloud) = self._localize_task('SpecificObject', [ bin, object ])

        # apply the camera transform
        xform = se3.mul(self.camera_xform, xform)
        cloud = map(lambda p: se3.apply(self.camera_xform, p[:3]) + [ p[3] ], cloud)

        debug_cloud(cloud, [ self.base_xform, self.camera_xform, xform ])

        return (xform, cloud)
Beispiel #2
0
    # set up the bin bounds
    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 ])
Beispiel #3
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')