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