def main(): logging.getLogger().setLevel(logging.INFO) # parse args parser = argparse.ArgumentParser(description='Register a webcam to the Photoneo PhoXi') parser.add_argument('--config_filename', type=str, default='cfg/tools/colorize_phoxi.yaml', help='filename of a YAML configuration for registration') args = parser.parse_args() config_filename = args.config_filename config = YamlConfig(config_filename) sensor_data = config['sensors'] phoxi_config = sensor_data['phoxi'] phoxi_config['frame'] = 'phoxi' # Initialize ROS node rospy.init_node('colorize_phoxi', anonymous=True) logging.getLogger().addHandler(rl.RosStreamHandler()) # Get PhoXi sensor set up phoxi = RgbdSensorFactory.sensor(phoxi_config['type'], phoxi_config) phoxi.start() # Capture PhoXi and webcam images phoxi_color_im, phoxi_depth_im, _ = phoxi.frames() # vis2d.figure() # vis2d.subplot(121) # vis2d.imshow(phoxi_color_im) # vis2d.subplot(122) # vis2d.imshow(phoxi_depth_im) # vis2d.show() phoxi_pc = phoxi.ir_intrinsics.deproject(phoxi_depth_im) colors = phoxi_color_im.data.reshape((phoxi_color_im.shape[0] * phoxi_color_im.shape[1], phoxi_color_im.shape[2])) / 255.0 vis3d.figure() vis3d.points(phoxi_pc.data.T[::3], color=colors[::3], scale=0.001) vis3d.show() # Export to PLY file vertices = phoxi.ir_intrinsics.deproject(phoxi_depth_im).data.T colors = phoxi_color_im.data.reshape(phoxi_color_im.data.shape[0] * phoxi_color_im.data.shape[1], phoxi_color_im.data.shape[2]) f = open('pcloud.ply', 'w') f.write('ply\nformat ascii 1.0\nelement vertex {}\nproperty float x\nproperty float y\nproperty float z\nproperty uchar red\n'.format(len(vertices)) + 'property uchar green\nproperty uchar blue\nend_header\n') for v, c in zip(vertices,colors): f.write('{} {} {} {} {} {}\n'.format(v[0], v[1], v[2], c[0], c[1], c[2])) f.close()
# form segmask segmask = depth_im_seg.to_binary() valid_px_segmask = depth_im.invalid_pixel_mask().inverse() segmask = segmask.mask_binary(valid_px_segmask) # inpaint color_im = raw_color_im.inpaint(rescale_factor=inpaint_rescale_factor) depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) return color_im, depth_im, segmask if __name__ == '__main__': # set up logger logging.getLogger().setLevel(logging.INFO) rospy.init_node('capture_dataset', anonymous=True) logging.getLogger().addHandler(rl.RosStreamHandler()) # parse args parser = argparse.ArgumentParser(description='Capture a dataset of RGB-D images from a set of sensors') parser.add_argument('output_dir', type=str, help='directory to save output') parser.add_argument('num_images', type=int, help='number of images to capture') parser.add_argument('--config_filename', type=str, default=None, help='path to configuration file to use') args = parser.parse_args() output_dir = args.output_dir num_images = args.num_images config_filename = args.config_filename # make output directory if not os.path.exists(output_dir): os.mkdir(output_dir)
from introspection import Introspector from runner import SMACHRunner ## used packages import rgoap import rospy ### set up rgoap-ros interface ## forward rgoap's logging to ROS import logging # ..for console output import rosgraph.roslogging as _rl logging.getLogger('rgoap').addHandler(_rl.RosStreamHandler()) # ..for network output (/rosout) import rospy.impl.rosout as _ro logging.getLogger('rgoap').addHandler(_ro.RosOutHandler()) # remove the default console output rgoap.remove_default_loghandler() ## shutdown handling rgoap.set_shutdown_check(rospy.is_shutdown)