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)
