def run_experiment(cap, disp, timeframes, timesteps, agents): # We're going to compare ourselves to the agents with the following IDs. print 'Running experiment...' util.reset_timer() agents_to_test = range(319, 331) # agents_to_test = [48, 49] true_paths = [] planned_paths = [] one_step_paths = [] # IGP_scores = np.zeros((len(agents_to_test), 2)) # ped_scores = np.zeros((len(agents_to_test), 2)) display.plot_prediction_metrics([], [], []) for i, agent in enumerate(agents_to_test): print 'Agent ', agent ped_path = agents[agent] path_length = ped_path.shape[0] start = 3 # Initializes path with start+1 points. final_path = np.zeros((path_length, 3)) final_path[0:start+1, :] = ped_path[0:start+1,1:4] one_step_final_path = final_path.copy() # Run IGP through the path sequence. for t in range(start, path_length-1): frame_num = timeframes[int(ped_path[t,0])] print 'doing frame', frame_num disp.set_frame(frame_num) final_path[t+1], one_step_final_path[t+1] = disp.do_frame(agent, final_path[:t+1], False, True) if cv2.waitKey(1) != -1: print 'Canceled!' return # Compute the final score for both IGP and pedestrian ground truth. print 'Agent', agent, 'done.' # start_time = int(ped_path[0,0]) # other_peds = [agents[a] for a in timesteps[start_time] if a != agent] # other_paths = [util.get_path_at_time(start_time, fullpath)[1][:,1:4] for fullpath in other_peds] # IGP_scores[i] = util.length_and_safety(final_path, other_paths) # ped_scores[i] = util.length_and_safety(ped_path[:,1:4], other_paths) true_paths.append(ped_path[:,1:4]) planned_paths.append(final_path) one_step_paths.append(one_step_final_path) pred_errs = util.calc_pred_scores(true_paths, planned_paths, util.prediction_errors) path_errs = util.calc_pred_scores(true_paths, planned_paths, util.path_errors) display.plot_prediction_metrics(pred_errs, path_errs, agents_to_test[0:i+1]) one_step_errors = util.calc_pred_scores(true_paths, one_step_paths, util.prediction_errors) pl.figure(2) pl.clf() display.plot_prediction_error('One-Step Prediction Errors', one_step_errors, agents_to_test[0:i+1]) pl.draw() # results = np.column_stack((agents_to_test, ped_scores, IGP_scores)) pred_results = np.column_stack((agents_to_test, pred_errs.T)) path_results = np.column_stack((agents_to_test, path_errs.T)) # np.savetxt('experiment.txt', results) np.savetxt('prediction_errors.txt', pred_results) np.savetxt('path_errors.txt', path_results) print 'EXPERIMENT COMPLETE.' util.report_time()
def run_experiment(cap, disp, timeframes, timesteps, agents): # We're going to compare ourselves to the agents with the following IDs. print 'Running experiment...' util.reset_timer() agents_to_test = range(319, 331) # agents_to_test = [48, 49] true_paths = [] planned_paths = [] one_step_paths = [] # IGP_scores = np.zeros((len(agents_to_test), 2)) # ped_scores = np.zeros((len(agents_to_test), 2)) display.plot_prediction_metrics([], [], []) for i, agent in enumerate(agents_to_test): print 'Agent ', agent ped_path = agents[agent] path_length = ped_path.shape[0] start = 3 # Initializes path with start+1 points. final_path = np.zeros((path_length, 3)) final_path[0:start + 1, :] = ped_path[0:start + 1, 1:4] one_step_final_path = final_path.copy() # Run IGP through the path sequence. for t in range(start, path_length - 1): frame_num = timeframes[int(ped_path[t, 0])] print 'doing frame', frame_num disp.set_frame(frame_num) final_path[t + 1], one_step_final_path[t + 1] = disp.do_frame( agent, final_path[:t + 1], False, True) if cv2.waitKey(1) != -1: print 'Canceled!' return # Compute the final score for both IGP and pedestrian ground truth. print 'Agent', agent, 'done.' # start_time = int(ped_path[0,0]) # other_peds = [agents[a] for a in timesteps[start_time] if a != agent] # other_paths = [util.get_path_at_time(start_time, fullpath)[1][:,1:4] for fullpath in other_peds] # IGP_scores[i] = util.length_and_safety(final_path, other_paths) # ped_scores[i] = util.length_and_safety(ped_path[:,1:4], other_paths) true_paths.append(ped_path[:, 1:4]) planned_paths.append(final_path) one_step_paths.append(one_step_final_path) pred_errs = util.calc_pred_scores(true_paths, planned_paths, util.prediction_errors) path_errs = util.calc_pred_scores(true_paths, planned_paths, util.path_errors) display.plot_prediction_metrics(pred_errs, path_errs, agents_to_test[0:i + 1]) one_step_errors = util.calc_pred_scores(true_paths, one_step_paths, util.prediction_errors) pl.figure(2) pl.clf() display.plot_prediction_error('One-Step Prediction Errors', one_step_errors, agents_to_test[0:i + 1]) pl.draw() # results = np.column_stack((agents_to_test, ped_scores, IGP_scores)) pred_results = np.column_stack((agents_to_test, pred_errs.T)) path_results = np.column_stack((agents_to_test, path_errs.T)) # np.savetxt('experiment.txt', results) np.savetxt('prediction_errors.txt', pred_results) np.savetxt('path_errors.txt', path_results) print 'EXPERIMENT COMPLETE.' util.report_time()
def do_frame(self, agent=-1, past_plan=None, with_scores=True, multi_prediction=False): if not self.cap.isOpened(): raise Exception('Video stream closed.') if agent == -1: agent = self.agent_num adex, displayed_agent = self.get_agent_index(agent) t_plus_one = None t_plus_one2 = None frame_num = int(self.cap.get(POS_FRAMES)) now = int(self.cap.get(POS_MSEC) / 1000) _, frame = self.cap.read() frame_txt = "{:0>2}:{:0>2}".format(now // 60, now % 60) agent_txt = 'Agent: {}'.format(displayed_agent) # Check for end of annotations. if frame_num >= len(self.frames): frame_txt += ' (eof)' agent_txt = '' else: frame_txt += ' (' + str(frame_num) + ')' t = self.frames[frame_num] # There really shouldn't be any empty timesteps but I found some so we'll have to deal with it. if t >= 0 and self.timesteps[t]: # If we've reached a new timestep, recompute the observations. if t != self.last_t: self.last_t = t adex, displayed_agent = self.get_agent_index(agent) agent_txt = 'Agent: {}'.format(displayed_agent) if self.do_predictions: self.predictions = util.make_predictions( t, self.timesteps, self.agents, self.model, agent, past_plan) # self.predictions = util.fake_predictions(t, self.timesteps, self.agents, 10.0) if multi_prediction and past_plan is not None: predictions2 = util.make_predictions( t, self.timesteps, self.agents, self.model, agent, None) if predictions2.plan[adex].shape[0] > 1: t_plus_one2 = predictions2.plan[adex][1] if self.predictions.plan[adex].shape[0] > 1: t_plus_one = self.predictions.plan[adex][1] if with_scores: # ped_scores, IGP_scores = util.calc_nav_scores(self.predictions.true_paths, self.predictions.plan) # plot_nav_metrics(ped_scores, IGP_scores) pred_errs = util.calc_pred_scores( self.predictions.true_paths, self.predictions.plan, util.prediction_errors) path_errs = util.calc_pred_scores( self.predictions.true_paths, self.predictions.plan, util.path_errors) plot_prediction_metrics(pred_errs, path_errs, self.timesteps[t]) else: self.predictions = util.get_past_paths( t, self.timesteps, self.agents) # Draw the obstacles. frame = np.maximum(frame, cv2.cvtColor(self.obs_map, cv2.COLOR_GRAY2BGR)) # Draw destinations. for d in self.destinations: d = np.append(d, 1) cv2.circle(frame, util.to_pixels(self.Hinv, d), 5, (0, 255, 0), -1) # Inform of frame and agent number. pt = (3, frame.shape[0] - 3) ll, ur = draw_text(frame, pt, frame_txt) if agent_txt: pt = (ll[0], ur[1]) ll, ur = draw_text(frame, pt, agent_txt) # Draw pedestrian paths so far. if self.draw_past and self.predictions.past: for path in self.predictions.past: draw_path(frame, path, (192, 192, 192)) draw_waypoints(frame, self.predictions.past[adex], (255, 211, 176)) # Draw predictions, if we have them. if self.predictions.plan: # For each agent, draw... peds_to_draw = range(len( self.predictions.plan)) if self.draw_all_agents else [adex] # The GP samples. if self.draw_samples != NO_SAMPLES: # What kind of sample are we showing? preds = self.predictions.prior if self.draw_samples == PRIOR_SAMPLES else self.predictions.posterior # Which sample(s) are we showing? if self.draw_all_samples: if preds: samples_to_draw = range(preds[0].shape[1]) else: samples_to_draw = [] else: sdex = self.sample_num samples_to_draw = [sdex] pt = (ll[0], ur[1]) ll, ur = draw_text(frame, pt, 'Sample: {}'.format(sdex + 1)) pt = (ll[0], ur[1]) draw_text( frame, pt, 'Weight: {:.1e}'.format( self.predictions.weights[sdex])) # Now actually draw the sample. for ddex in peds_to_draw: for i in samples_to_draw: path = preds[ddex][:, i, :] path = np.column_stack((path, np.ones(path.shape[0]))) draw_path(frame, path, (255, 0, 0)) # The ground truth. if self.draw_truth: for ddex in peds_to_draw: draw_path(frame, self.predictions.true_paths[ddex], (0, 255, 0)) # The final prediction. if self.draw_plan: for ddex in peds_to_draw: draw_path(frame, self.predictions.plan[ddex], (0, 192, 192)) if not self.draw_all_agents and past_plan is not None: draw_path(frame, past_plan, (0, 192, 192)) draw_waypoints(frame, past_plan, (0, 192, 192)) cv2.imshow('frame', frame) return t_plus_one if t_plus_one2 is None else (t_plus_one, t_plus_one2)
def do_frame(self, agent=-1, past_plan=None, with_scores=True, multi_prediction=False): if not self.cap.isOpened(): raise Exception('Video stream closed.') if agent == -1: agent = self.agent_num adex, displayed_agent = self.get_agent_index(agent) t_plus_one = None t_plus_one2 = None frame_num = int(self.cap.get(POS_FRAMES)) now = int(self.cap.get(POS_MSEC) / 1000) _, frame = self.cap.read() frame_txt = "{:0>2}:{:0>2}".format(now//60, now%60) agent_txt = 'Agent: {}'.format(displayed_agent) # Check for end of annotations. if frame_num >= len(self.frames): frame_txt += ' (eof)' agent_txt = '' else: frame_txt += ' (' + str(frame_num) + ')' t = self.frames[frame_num] # There really shouldn't be any empty timesteps but I found some so we'll have to deal with it. if t >= 0 and self.timesteps[t]: # If we've reached a new timestep, recompute the observations. if t != self.last_t: self.last_t = t adex, displayed_agent = self.get_agent_index(agent) agent_txt = 'Agent: {}'.format(displayed_agent) if self.do_predictions: self.predictions = util.make_predictions(t, self.timesteps, self.agents, self.model, agent, past_plan) # self.predictions = util.fake_predictions(t, self.timesteps, self.agents, 10.0) if multi_prediction and past_plan is not None: predictions2 = util.make_predictions(t, self.timesteps, self.agents, self.model, agent, None) if predictions2.plan[adex].shape[0] > 1: t_plus_one2 = predictions2.plan[adex][1] if self.predictions.plan[adex].shape[0] > 1: t_plus_one = self.predictions.plan[adex][1] if with_scores: # ped_scores, IGP_scores = util.calc_nav_scores(self.predictions.true_paths, self.predictions.plan) # plot_nav_metrics(ped_scores, IGP_scores) pred_errs = util.calc_pred_scores(self.predictions.true_paths, self.predictions.plan, util.prediction_errors) path_errs = util.calc_pred_scores(self.predictions.true_paths, self.predictions.plan, util.path_errors) plot_prediction_metrics(pred_errs, path_errs, self.timesteps[t]) else: self.predictions = util.get_past_paths(t, self.timesteps, self.agents) # Draw the obstacles. frame = np.maximum(frame, cv2.cvtColor(self.obs_map, cv2.COLOR_GRAY2BGR)) # Draw destinations. for d in self.destinations: d = np.append(d, 1) cv2.circle(frame, util.to_pixels(self.Hinv, d), 5, (0,255,0), -1) # Inform of frame and agent number. pt = (3, frame.shape[0]-3) ll, ur = draw_text(frame, pt, frame_txt) if agent_txt: pt = (ll[0], ur[1]) ll, ur = draw_text(frame, pt, agent_txt) # Draw pedestrian paths so far. if self.draw_past and self.predictions.past: for path in self.predictions.past: draw_path(frame, path, (192,192,192)) draw_waypoints(frame, self.predictions.past[adex], (255,211,176)) # Draw predictions, if we have them. if self.predictions.plan: # For each agent, draw... peds_to_draw = range(len(self.predictions.plan)) if self.draw_all_agents else [adex] # The GP samples. if self.draw_samples != NO_SAMPLES: # What kind of sample are we showing? preds = self.predictions.prior if self.draw_samples == PRIOR_SAMPLES else self.predictions.posterior # Which sample(s) are we showing? if self.draw_all_samples: if preds: samples_to_draw = range(preds[0].shape[1]) else: samples_to_draw = [] else: sdex = self.sample_num samples_to_draw = [sdex] pt = (ll[0], ur[1]) ll, ur = draw_text(frame, pt, 'Sample: {}'.format(sdex+1)) pt = (ll[0], ur[1]) draw_text(frame, pt, 'Weight: {:.1e}'.format(self.predictions.weights[sdex])) # Now actually draw the sample. for ddex in peds_to_draw: for i in samples_to_draw: path = preds[ddex][:,i,:] path = np.column_stack((path, np.ones(path.shape[0]))) draw_path(frame, path, (255,0,0)) # The ground truth. if self.draw_truth: for ddex in peds_to_draw: draw_path(frame, self.predictions.true_paths[ddex], (0,255,0)) # The final prediction. if self.draw_plan: for ddex in peds_to_draw: draw_path(frame, self.predictions.plan[ddex], (0,192,192)) if not self.draw_all_agents and past_plan is not None: draw_path(frame, past_plan, (0,192,192)) draw_waypoints(frame, past_plan, (0,192,192)) cv2.imshow('frame', frame) return t_plus_one if t_plus_one2 is None else (t_plus_one, t_plus_one2)