def run(obs_num, img, init_phi, init_weight): state = (img[:, :, 0] != 255) | (img[:, :, 1] != 255) | (img[:, :, 2] != 255) tsdf, sdf, depth, weight, always_trust_mask = \ observation.observation_from_full_img(img, tracker_params) new_phi, new_weight, problem_data = timb.run_one_step( gp, tracker_params, tsdf, weight, always_trust_mask, init_phi, init_weight, return_full=True) trusted = timb.threshold_trusted(tracker_params, new_phi, new_weight) output = np.where(trusted, new_phi, np.nan) timb.plot_problem_data(plt, tracker_params.tsdf_trunc_dist, gp, state, tsdf, weight, init_phi, init_weight, problem_data['result'], problem_data['opt_result'], new_phi, new_weight, output) if args.output_dir is None: plt.show() else: plt.savefig('%s/plots_%d.png' % (args.output_dir, obs_num), bbox_inches='tight') if args.dump_dir is not None: import cPickle path = '%s/dump_%d.pkl' % (args.dump_dir, obs_num) with open(path, 'w') as f: cPickle.dump(problem_data, f, cPickle.HIGHEST_PROTOCOL) print 'wrote to', path return new_phi, new_weight
def run(obs_num, img, init_phi, init_weight): state = (img[:,:,0] != 255) | (img[:,:,1] != 255) | (img[:,:,2] != 255) tsdf, sdf, depth, weight, always_trust_mask = \ observation.observation_from_full_img(img, tracker_params) new_phi, new_weight, problem_data = timb.run_one_step( gp, tracker_params, tsdf, weight, always_trust_mask, init_phi, init_weight, return_full=True ) trusted = timb.threshold_trusted(tracker_params, new_phi, new_weight) output = np.where(trusted, new_phi, np.nan) timb.plot_problem_data( plt, tracker_params.tsdf_trunc_dist, gp, state, tsdf, weight, init_phi, init_weight, problem_data['result'], problem_data['opt_result'], new_phi, new_weight, output ) if args.output_dir is None: plt.show() else: plt.savefig('%s/plots_%d.png' % (args.output_dir, obs_num), bbox_inches='tight') if args.dump_dir is not None: import cPickle path = '%s/dump_%d.pkl' % (args.dump_dir, obs_num) with open(path, 'w') as f: cPickle.dump(problem_data, f, cPickle.HIGHEST_PROTOCOL) print 'wrote to', path return new_phi, new_weight
rigid_tracker.plot_problem_data( plt, tracker_params.tsdf_trunc_dist, grid_params, entry['state'] if 'state' in entry else np.zeros_like(entry['obs_tsdf']), np.empty((0,2)), entry['obs_tsdf'], entry['obs_weight'], entry['curr_phi'], entry['curr_weight'], entry['new_phi'], entry['new_weight'], output ) else: timb.plot_problem_data( plt, tracker_params.tsdf_trunc_dist, grid_params, entry['state'] if 'state' in entry else np.zeros_like(entry['obs_tsdf']), entry['obs_tsdf'], entry['obs_weight'], entry['curr_phi'], entry['curr_weight'], entry['problem_data']['result'], entry['problem_data']['opt_result'], entry['new_phi'], entry['new_weight'], output ) if args.dump_to_dir is not None: matplotlib.use('Agg') if use_bar: bar.next() else: print i plt.savefig(os.path.join(args.dump_to_dir, '%05d.png' % i), bbox_inches='tight') i += 1 elif args.dump_to_mongo: