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 load_demo(taskname='overhand_knot', demos_list_file='knot_demos.yaml', seg_name=None):
data_dir = os.path.join(os.path.dirname(lfd.__file__), "data")
with open(os.path.join(data_dir, demos_list_file),"r") as fh:
task_info = yaml.load(fh)
H5FILE = os.path.join(data_dir, task_info[taskname]["db_file"])
demos_file = h5py.File(H5FILE,"r")
demos = warping.group_to_dict(demos_file)
demos_file.close()
if seg_name is None:
seg_name = select_from_list(sorted(demos.keys()))
demo = demos[seg_name]
demo["cloud_xyz_ds"], ds_inds = recognition.downsample(demo["cloud_xyz"])
demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"])
return demo
def load_demo(taskname='overhand_knot',
demos_list_file='knot_demos.yaml',
seg_name=None):
data_dir = os.path.join(os.path.dirname(lfd.__file__), "data")
with open(os.path.join(data_dir, demos_list_file), "r") as fh:
task_info = yaml.load(fh)
H5FILE = os.path.join(data_dir, task_info[taskname]["db_file"])
demos_file = h5py.File(H5FILE, "r")
demos = warping.group_to_dict(demos_file)
demos_file.close()
if seg_name is None:
seg_name = select_from_list(sorted(demos.keys()))
demo = demos[seg_name]
demo["cloud_xyz_ds"], ds_inds = recognition.downsample(demo["cloud_xyz"])
demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"])
return demo
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())), )
