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
def get_rigid_observation(self, i, npad): return observation_rigid.observation_from_full_state_rigid(self.get_padded_state(i, npad), self.tracker_params)
