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
def run_experiment(ex, tracker_params, callback=None, iter_cap=None): assert isinstance(tracker_params, timb.TrackerParams) ex.set_tracker_params(tracker_params) grid_params = ex.get_grid_params() curr_phi, curr_weight = ex.get_prior() experiment_log = [] num = ex.num_observations() if iter_cap is None else min(ex.num_observations(), iter_cap) for i in range(num): iter_data = {} iter_data['curr_phi'], iter_data['curr_weight'] = curr_phi, curr_weight obs_tsdf, obs_sdf, obs_depth, obs_weight, obs_trust_mask = ex.get_observation(i) iter_data['state'] = ex.get_state(i) iter_data['obs_tsdf'], iter_data['obs_sdf'], iter_data['obs_depth'], iter_data['obs_weight'], iter_data['obs_trust_mask'] = obs_tsdf, obs_sdf, obs_depth, obs_weight, obs_trust_mask new_phi, new_weight, problem_data = timb.run_one_step( grid_params, tracker_params, obs_tsdf, obs_weight, obs_trust_mask, curr_phi, curr_weight, return_full=True ) iter_data['new_phi'], iter_data['new_weight'], iter_data['problem_data'] = new_phi, new_weight, problem_data # iter_data['trusted'] = trusted = timb.threshold_trusted(tracker_params, new_phi, new_weight) # iter_data['output'] = np.where(trusted, new_phi, np.nan) if callback is not None: callback(i, iter_data) experiment_log.append(iter_data) curr_phi, curr_weight = new_phi, new_weight return experiment_log