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)
# 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 ])
# 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')
