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
