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 run_experiment_rigid(ex, rigid_tracker_params, callback=None, iter_cap=None):
assert isinstance(rigid_tracker_params, rigid_tracker.RigidTrackerParams)
ex.set_tracker_params(rigid_tracker_params)
#grid_params = ex.get_grid_params()
# TODO: doesn't work for arbitrary sizes
NPAD = 0
SIZE = 100
PSIZE = SIZE + 2*NPAD
WORLD_MIN = (0., 0.)
WORLD_MAX = (PSIZE-1., PSIZE-1.)
padded_grid_params = timb.GridParams(WORLD_MIN[0], WORLD_MAX[0], WORLD_MIN[1], WORLD_MAX[1], PSIZE, PSIZE)
padded_curr_phi, padded_curr_weight = ex.get_prior(size=PSIZE)
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 = {}
curr_phi = rigid_tracker.unpad_state(padded_curr_phi, NPAD, SIZE, SIZE)
curr_weight = rigid_tracker.unpad_state(padded_curr_weight, NPAD, SIZE, SIZE)
iter_data['curr_phi'], iter_data['curr_weight'] = curr_phi, curr_weight
padded_obs_tsdf, padded_obs_sdf, padded_obs_depth, padded_obs_weight = ex.get_rigid_observation(i, NPAD)
iter_data['state'] = ex.get_state(i)
iter_data['obs_tsdf'], iter_data['obs_sdf'], iter_data['obs_depth'], iter_data['obs_weight'] = \
rigid_tracker.unpad_state(padded_obs_tsdf, NPAD, SIZE, SIZE), \
rigid_tracker.unpad_state(padded_obs_sdf, NPAD, SIZE, SIZE), \
padded_obs_depth[NPAD:NPAD+SIZE] - NPAD, \
rigid_tracker.unpad_state(padded_obs_weight, NPAD, SIZE, SIZE) #TODO: unpad for viewing?
padded_new_phi, padded_new_weight, padded_obs_xy, problem_data = rigid_tracker.run_one_rigid_step(
padded_grid_params, rigid_tracker_params,
padded_obs_depth, padded_obs_tsdf, padded_obs_weight,
padded_curr_phi, padded_curr_weight,
return_full=True
)
new_phi = rigid_tracker.unpad_state(padded_new_phi, NPAD, SIZE, SIZE)
new_weight = rigid_tracker.unpad_state(padded_new_weight, NPAD, SIZE, SIZE)
iter_data['new_phi'], iter_data['new_weight'], iter_data['problem_data'] = new_phi, new_weight, problem_data
if callback is not None:
callback(i, iter_data)
experiment_log.append(iter_data)
padded_curr_phi, padded_curr_weight = padded_new_phi, padded_new_weight
return experiment_log
