def main():
viewer = miniviewer.make_viewer()
# Define point cloud-reading stream, depending on input method
if args.input is None:
live = True
print 'No input file specified, streaming clouds live'
def stream_rgbd():
import rospy
from rapprentice import berkeley_pr2, PR2
rospy.init_node("tracker", disable_signals=True)
pr2 = PR2.PR2()
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
while True:
print 'Grabbing cloud...'
rgb, depth = grabber.getRGBD()
pr2.update_rave()
T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot)
print 'done'
yield rgb, depth, T_w_k
else:
live = False
print 'Reading input file', args.input
import h5py
def stream_rgbd():
h5 = h5py.File(args.input, 'r')
rgbs, depths, T_w_k = h5['rgb'], h5['depth'], np.array(h5['T_w_k'])
num_frames = len(rgbs)
skip = 2
start = 160
for i_frame in range(start, num_frames, skip):
print 'Processing frame %d/%d.' % (i_frame + 1, num_frames)
yield rgbs[i_frame], depths[i_frame], T_w_k
# Tracking loop: run EM iterations for each point cloud received
tracked_obj = None
for rgb, depth, T_w_k in stream_rgbd():
mask_func = clouds.yellow_mask if args.obj_type == 'suture_pad' else clouds.red_mask
cloud_xyz = clouds.extract_color(rgb, depth, T_w_k, mask_func)
if len(cloud_xyz) == 0:
print 'Filtered cloud is empty. Skipping frame.'
continue
# Initialization: on the first frame, create cloth and table
if tracked_obj is None:
init_handles = []
tracked_obj = initialize_tracked_obj(viewer, init_handles,
cloud_xyz)
print 'Initialization ok?'
handles = []
plot(viewer, handles, cloud_xyz, T_w_k, tracked_obj)
#viewer.Idle()
if args.gpu:
from trackingpy import tracking_gpu
tracker = tracking_gpu.GPUTracker(tracked_obj)
else:
tracker = tracking.Tracker(tracked_obj)
tracker.set_input(cloud_xyz, depth, T_w_k)
out = tracker.step(return_data=True)
handles = []
plot(viewer, handles, cloud_xyz, T_w_k, tracked_obj,
out['occluded_in_depth_img'], out['occluded_by_model'],
out['force_kd'])
if not live:
raw_input('Press enter for the next frame')
def main():
parser = argparse.ArgumentParser()
parser.add_argument('file', type=str)
parser.add_argument('--real_robot', action='store_true')
parser.add_argument('--view', action='store_true')
args = parser.parse_args()
if args.view:
print 'Opening file %s' % args.file
f = h5py.File(args.file, 'r')
rgbs = f['rgb']
depths = f['depth']
T_w_k = np.array(f['T_w_k'])
Globals.env = openravepy.Environment()
viewer = trajoptpy.GetViewer(Globals.env)
num_frames = len(rgbs)
for i in range(0, num_frames, 10):
print 'Showing frame %d/%d' % (i+1, num_frames)
rgb, depth = rgbs[i], depths[i]
# XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
# XYZ_w = XYZ_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
# handle = Globals.env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.)
xyz = clouds.extract_color(rgb, depth, T_w_k, clouds.yellow_mask)
handles = []
handles.append(Globals.env.plot3(xyz, 2, (1,0,0)))
draw_ax(T_w_k, .1, Globals.env, handles)
viewer.Idle()
return
if args.real_robot:
import rospy
rospy.init_node("recorder", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
Globals.pr2.update_rave()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
else:
Globals.env = openravepy.Environment()
T_w_k = np.eye(4)
viewer = trajoptpy.GetViewer(Globals.env)
#env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05]))
num_frames = 0
rgbs, depths = [], []
subprocess.call("killall XnSensorServer", shell=True)
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
try:
while True:
rgb, depth = grabber.getRGBD()
rgbs.append(rgb)
depths.append(depth)
cv2.imshow("rgb", rgb)
cv2.imshow("depth", cmap[np.fmin((depth*.064).astype('int'), 255)])
cv2.waitKey(30)
num_frames += 1
print 'Captured frame', num_frames
viewer.Step()
except KeyboardInterrupt:
print 'Keyboard interrupt'
print 'Writing to %s ...' % args.file
out = h5py.File(args.file, 'w')
out.create_dataset('rgb', data=rgbs, compression='gzip', chunks=(1, 256, 256, 3))
out.create_dataset('depth', data=depths, compression='gzip', chunks=(1, 256, 256))
out['T_w_k'] = T_w_k
out.close()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('file', type=str)
parser.add_argument('--real_robot', action='store_true')
parser.add_argument('--view', action='store_true')
args = parser.parse_args()
if args.view:
print 'Opening file %s' % args.file
f = h5py.File(args.file, 'r')
rgbs = f['rgb']
depths = f['depth']
T_w_k = np.array(f['T_w_k'])
Globals.env = openravepy.Environment()
viewer = trajoptpy.GetViewer(Globals.env)
num_frames = len(rgbs)
for i in range(0, num_frames, 10):
print 'Showing frame %d/%d' % (i + 1, num_frames)
rgb, depth = rgbs[i], depths[i]
# XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
# XYZ_w = XYZ_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
# handle = Globals.env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.)
xyz = clouds.extract_color(rgb, depth, T_w_k, clouds.yellow_mask)
handles = []
handles.append(Globals.env.plot3(xyz, 2, (1, 0, 0)))
draw_ax(T_w_k, .1, Globals.env, handles)
viewer.Idle()
return
if args.real_robot:
import rospy
rospy.init_node("recorder", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
Globals.pr2.update_rave()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
else:
Globals.env = openravepy.Environment()
T_w_k = np.eye(4)
viewer = trajoptpy.GetViewer(Globals.env)
#env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05]))
num_frames = 0
rgbs, depths = [], []
subprocess.call("killall XnSensorServer", shell=True)
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
try:
while True:
rgb, depth = grabber.getRGBD()
rgbs.append(rgb)
depths.append(depth)
cv2.imshow("rgb", rgb)
cv2.imshow("depth", cmap[np.fmin((depth * .064).astype('int'),
255)])
cv2.waitKey(30)
num_frames += 1
print 'Captured frame', num_frames
viewer.Step()
except KeyboardInterrupt:
print 'Keyboard interrupt'
print 'Writing to %s ...' % args.file
out = h5py.File(args.file, 'w')
out.create_dataset('rgb',
data=rgbs,
compression='gzip',
chunks=(1, 256, 256, 3))
out.create_dataset('depth',
data=depths,
compression='gzip',
chunks=(1, 256, 256))
out['T_w_k'] = T_w_k
out.close()
def main():
viewer = miniviewer.make_viewer()
# Define point cloud-reading stream, depending on input method
if args.input is None:
live = True
print 'No input file specified, streaming clouds live'
def stream_rgbd():
import rospy
from rapprentice import berkeley_pr2, PR2
rospy.init_node("tracker", disable_signals=True)
pr2 = PR2.PR2()
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
while True:
print 'Grabbing cloud...'
rgb, depth = grabber.getRGBD()
pr2.update_rave()
T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot)
print 'done'
yield rgb, depth, T_w_k
else:
live = False
print 'Reading input file', args.input
import h5py
def stream_rgbd():
h5 = h5py.File(args.input, 'r')
rgbs, depths, T_w_k = h5['rgb'], h5['depth'], np.array(h5['T_w_k'])
num_frames = len(rgbs)
skip = 2
start = 160
for i_frame in range(start, num_frames, skip):
print 'Processing frame %d/%d.' % (i_frame+1, num_frames)
yield rgbs[i_frame], depths[i_frame], T_w_k
# Tracking loop: run EM iterations for each point cloud received
tracked_obj = None
for rgb, depth, T_w_k in stream_rgbd():
mask_func = clouds.yellow_mask if args.obj_type == 'suture_pad' else clouds.red_mask
cloud_xyz = clouds.extract_color(rgb, depth, T_w_k, mask_func)
if len(cloud_xyz) == 0:
print 'Filtered cloud is empty. Skipping frame.'
continue
# Initialization: on the first frame, create cloth and table
if tracked_obj is None:
init_handles = []
tracked_obj = initialize_tracked_obj(viewer, init_handles, cloud_xyz)
print 'Initialization ok?'
handles = []; plot(viewer, handles, cloud_xyz, T_w_k, tracked_obj)
#viewer.Idle()
if args.gpu:
from trackingpy import tracking_gpu
tracker = tracking_gpu.GPUTracker(tracked_obj)
else:
tracker = tracking.Tracker(tracked_obj)
tracker.set_input(cloud_xyz, depth, T_w_k)
out = tracker.step(return_data=True)
handles = []; plot(viewer, handles, cloud_xyz, T_w_k, tracked_obj, out['occluded_in_depth_img'], out['occluded_by_model'], out['force_kd'])
if not live:
raw_input('Press enter for the next frame')
