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:
