def evaluate_obstacles(args):
# Array with columns (x, y).
ground_obst = read_csv(os.path.join(
args.eval_data_dir, 'eval_obstacles.csv'))
# Array with columns (x, y).
out_obst = read_csv(os.path.join(args.out_dir, 'slam_out_landmarks.csv'))
# Use evaluation metric written by Galois.
n_extras, score = obsticle_compare(out_obst, ground_obst)
print "{} ground obstacles; {} out obstacles".format(
len(ground_obst), len(out_obst))
print "Map error: {}".format(score)
# Optionally plot obstacles.
if args.plot:
r = 0.37
ground_obst_3 = [(x, y, r) for x, y in ground_obst]
out_obst_3 = [(x, y, r) for x, y in out_obst]
plot_obstacles(ground_obst_3, plt.gca(), color=(0, 1, 0, 0.6))
plot_obstacles(out_obst_3, plt.gca(), color=(1, 0, 0, 0.6))
def __init__(self, dataset, num_particles):
super(LocPF, self).__init__(num_particles)
self.dataset = dataset
self.current_velocity = 0.0
self.current_steering = 0.0
self.add_controls_noise = False
self.velocity_noise_stdev = 0.1
self.steering_noise_stdev = 0.3
self.add_dynamics_noise = True
self.x_noise_stdev = 0.003
self.y_noise_stdev = 0.003
self.theta_noise_stdev = 0.0015
self.obs_cov_scale = 30.0
self.laser_angles = lasers.default_laser_angles()
self.laser_max_range = lasers.default_laser_max_range()
# Convert obstacles from list of (x, y) to array of (x, y, r).
obstacle_radius = 0.37
self.ground_obstacles = []
for x, y in dataset.ground_obstacles:
self.ground_obstacles.append((x, y, obstacle_radius))
self.ground_obstacles = np.array(self.ground_obstacles)
self.fig1 = plt.figure(figsize=(8, 12))
self.ax1 = self.fig1.add_subplot(211)
self.ax2 = self.fig1.add_subplot(413)
self.ax3 = self.fig1.add_subplot(414)
# Hack to display colorbar for angle.
plt.sca(self.ax1)
a = np.array([[-np.pi, np.pi]])
x = plt.pcolor(a)
self.ax1.clear()
plt.colorbar(x, ax=self.ax1)
# Hack to display legends for the bottom plots.
dummy_lines = []
dummy_lines += self.ax2.plot(
[0, 0], [0, 0], 'g', label='ground_gps_llik')
dummy_lines += self.ax2.plot(
[0, 0], [0, 0], 'r', label='best_particle_llik')
self.ax2.legend()
dummy_lines += self.ax3.plot([0, 0], [0, 0], label='error^2')
self.ax3.legend()
for line in dummy_lines:
line.remove()
# Plot ground-truth and dead-reckoning trajectories.
gps_traj = draw_dr.get_ground_truth_traj(dataset)
self.ax1.plot(gps_traj[:, 1], gps_traj[:, 2], label='ground')
dr_traj = draw_dr.get_dead_reckoning_traj(dataset, draw_dr.dynamics)
self.ax1.plot(dr_traj[:, 1], dr_traj[:, 2], label='dead-reckoning')
lasers.plot_obstacles(self.ground_obstacles, self.ax1)
self.ax1.set_xlim(X_MIN - 1, X_MAX + 1)
self.ax1.set_ylim(Y_MIN - 1, Y_MAX + 1)
self.ax1.legend()
plt.ion() # needed for running through cProfile
plt.show()
