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)
