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)