-
Notifications
You must be signed in to change notification settings - Fork 33
/
colorize_phoxi.py
70 lines (57 loc) · 2.39 KB
/
colorize_phoxi.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
"""
Script to register a webcam to the Photoneo PhoXi
Author: Matt Matl
"""
import argparse
import logging
import numpy as np
import os
import plyfile
import time
import rospy
import rosgraph.roslogging as rl
from autolab_core import YamlConfig, RigidTransform, PointCloud
from perception import RgbdSensorFactory, ColorImage
from visualization import Visualizer2D as vis2d
from visualization import Visualizer3D as vis3d
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()
if __name__ == '__main__':
main()