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 )