def run(obs_num, img, init_phi, init_weight):
state = ((img[:,:,0] != 255) | (img[:,:,1] != 255) | (img[:,:,2] != 255)).astype('float64')
state = pad_state(state, NPAD)
print state.shape
# plt.subplot(211)
# plt.imshow(state)
# plt.subplot(212)
# plt.imshow(pstate)
# plt.show()
tsdf, sdf, depth, w = robs.observation_from_full_state_rigid(state, tracker_params)
## optimize for camera pose and find the new sdf:
new_phi, new_weight, obs_xy, problem_data = rt.run_one_rigid_step(gp, tracker_params, depth, tsdf, w, init_phi, init_weight, return_full=True)
trusted = rt.threshold_trusted_for_view(new_weight)
out_state = np.where(trusted, new_phi, np.nan)
rt.plot_problem_data(plt, TSDF_TRUNC, gp, state, obs_xy, tsdf, w, init_phi, init_weight, new_phi, new_weight, out_state)
print problem_data['opt_result']['x']
if args.output_dir is None and obs_num%10==0:
plt.show()
else:
pass
#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
if use_bar:
bar = Bar(max=len(log))
while i < len(log):
entry = log[i]
trusted = timb.threshold_trusted_for_view(tracker_params, entry['new_phi'], entry['new_weight'])
output = np.where(trusted, entry['new_phi'], np.nan)
if data['rigid']:
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
)
