def main(): import argparse parser = argparse.ArgumentParser() parser.add_argument('clouds', nargs='+', help='cloud resource srings') args = parser.parse_args() assert len(args.clouds) == 2 import cloud_io print 'loading clouds' clouds = [cloud_io.read_from_rsrc(c) for c in args.clouds] print 'processing' clouds_ds = [recognition.downsample(c)[0] for c in clouds] # hists_ds = [recognition.calc_shape_context_hists(dc) for dc in clouds_ds] f, partners, hists0, hists1, costs = recognition.match_and_calc_shape_context(clouds_ds[0], clouds_ds[1], return_tuple=True) left_data = { 'xyz': clouds[0], 'xyz_ds': clouds_ds[0], 'hists_ds': hists0 } right_data = { 'xyz': clouds[1], 'xyz_ds': clouds_ds[1], 'hists_ds': hists1 } corr_data = { 'f': f, 'partners': partners, 'costs': costs } print 'drawing' draw_comparison(left_data, right_data, corr_data)
def main(): import argparse parser = argparse.ArgumentParser() parser.add_argument('clouds', nargs='+', help='cloud resource srings') args = parser.parse_args() assert len(args.clouds) == 2 import cloud_io print 'loading clouds' clouds = [cloud_io.read_from_rsrc(c) for c in args.clouds] print 'processing' clouds_ds = [recognition.downsample(c)[0] for c in clouds] # hists_ds = [recognition.calc_shape_context_hists(dc) for dc in clouds_ds] f, partners, hists0, hists1, costs = recognition.match_and_calc_shape_context( clouds_ds[0], clouds_ds[1], return_tuple=True) left_data = {'xyz': clouds[0], 'xyz_ds': clouds_ds[0], 'hists_ds': hists0} right_data = {'xyz': clouds[1], 'xyz_ds': clouds_ds[1], 'hists_ds': hists1} corr_data = {'f': f, 'partners': partners, 'costs': costs} print 'drawing' draw_comparison(left_data, right_data, corr_data)
def warping_loop(args, take_snapshot, quit): if rospy.get_name() == '/unnamed': rospy.init_node('publish_single_cloud', disable_signals=True) Globals.setup() demo = load_demo(args.taskname, args.demos_list_file, args.seg_name) imarker_server = None if args.warp_type == 'rigid_interactive': from interactive_markers.interactive_marker_server import * init_pos = demo['cloud_xyz'].mean(axis=0)[:3] marker_6dof = utils_rviz.make_interactive_marker_6dof(init_pos, args.frame) imarker_server = InteractiveMarkerServer('demo_pose_control') def callback_fn(feedback): xyz = feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z imarker_server.insert(marker_6dof, callback_fn) imarker_server.applyChanges() # align demo to test in a loop #prev_params = None keep_warping = True num_saved = 0 while not quit.value: xyz_new = load_cloud_from_sensor(args.input_topic, args.frame) if xyz_new.size == 0: continue xyz_demo_ds = demo['cloud_xyz_ds'] xyz_new_ds = recognition.downsample(xyz_new)[0] if keep_warping: f = fit_warp(args, xyz_demo_ds, xyz_new_ds, set_warp_params=args.set_warp_params) if hasattr(f, 'get_params') and args.set_warp_params is None: print 'fitted warp params: "%s"' % (' '.join(map(str, f.get_params())), ) if args.warp_once_only: keep_warping = False if args.show_shape_contexts: shape_context_costs = recognition.match_and_calc_shape_context(xyz_demo_ds, xyz_new_ds, normalize_costs=True) colors = [(c, 1-c, 0, 1) for c in shape_context_costs] else: import itertools colors = itertools.repeat((1, 0, 0, 1)) warped_pose_array = conversions.array_to_pose_array(f.transform_points(xyz_demo_ds), args.frame) Globals.handles = [] import geometry_msgs.msg as gm for p, rgba in zip(warped_pose_array.poses, colors): ps = gm.PoseStamped() ps.header = warped_pose_array.header ps.pose = p Globals.handles.append(Globals.rviz.draw_marker(ps, scale=(.01, .01, .01), rgba=rgba)) if take_snapshot.value: filename = '%s%04d' % (args.save_prefix, num_saved) print 'Saving snapshot to %s' % (filename,) take_snapshot.value = False finv = fit_warp(args, xyz_new_ds, xyz_demo_ds, set_warp_params=args.set_inv_warp_params) xyz_new_ds_out = finv.transform_points(xyz_new_ds) xyz_new_out = finv.transform_points(xyz_new) np.savez(filename, cloud_xyz=xyz_new_out, cloud_xyz_ds=xyz_new_ds_out) num_saved += 1 print 'Done saving snapshot. (used inverse transformation "%s")' % (' '.join(map(str, finv.get_params())), )
def warping_loop(args, take_snapshot, quit): if rospy.get_name() == '/unnamed': rospy.init_node('publish_single_cloud', disable_signals=True) Globals.setup() demo = load_demo(args.taskname, args.demos_list_file, args.seg_name) imarker_server = None if args.warp_type == 'rigid_interactive': from interactive_markers.interactive_marker_server import * init_pos = demo['cloud_xyz'].mean(axis=0)[:3] marker_6dof = utils_rviz.make_interactive_marker_6dof( init_pos, args.frame) imarker_server = InteractiveMarkerServer('demo_pose_control') def callback_fn(feedback): xyz = feedback.pose.position.x, feedback.pose.position.y, feedback.pose.position.z imarker_server.insert(marker_6dof, callback_fn) imarker_server.applyChanges() # align demo to test in a loop #prev_params = None keep_warping = True num_saved = 0 while not quit.value: xyz_new = load_cloud_from_sensor(args.input_topic, args.frame) if xyz_new.size == 0: continue xyz_demo_ds = demo['cloud_xyz_ds'] xyz_new_ds = recognition.downsample(xyz_new)[0] if keep_warping: f = fit_warp(args, xyz_demo_ds, xyz_new_ds, set_warp_params=args.set_warp_params) if hasattr(f, 'get_params') and args.set_warp_params is None: print 'fitted warp params: "%s"' % (' '.join( map(str, f.get_params())), ) if args.warp_once_only: keep_warping = False if args.show_shape_contexts: shape_context_costs = recognition.match_and_calc_shape_context( xyz_demo_ds, xyz_new_ds, normalize_costs=True) colors = [(c, 1 - c, 0, 1) for c in shape_context_costs] else: import itertools colors = itertools.repeat((1, 0, 0, 1)) warped_pose_array = conversions.array_to_pose_array( f.transform_points(xyz_demo_ds), args.frame) Globals.handles = [] import geometry_msgs.msg as gm for p, rgba in zip(warped_pose_array.poses, colors): ps = gm.PoseStamped() ps.header = warped_pose_array.header ps.pose = p Globals.handles.append( Globals.rviz.draw_marker(ps, scale=(.01, .01, .01), rgba=rgba)) if take_snapshot.value: filename = '%s%04d' % (args.save_prefix, num_saved) print 'Saving snapshot to %s' % (filename, ) take_snapshot.value = False finv = fit_warp(args, xyz_new_ds, xyz_demo_ds, set_warp_params=args.set_inv_warp_params) xyz_new_ds_out = finv.transform_points(xyz_new_ds) xyz_new_out = finv.transform_points(xyz_new) np.savez(filename, cloud_xyz=xyz_new_out, cloud_xyz_ds=xyz_new_ds_out) num_saved += 1 print 'Done saving snapshot. (used inverse transformation "%s")' % ( ' '.join(map(str, finv.get_params())), )