def test_readings_for_obstacles_regression_1():
"""
Regression test for bug before normalize_radians().
"""
a = 0.299541
b = 0.0500507
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
obstacles = np.array([
[-4.475, 1.45, 0.35],
[-1.3, 1.025, 0.35],
[-3., -1.55, 0.35],
[0.65, -1.95, 0.35],
[-1.95, -3.8, 0.35],
[0.15, -5.625, 0.35]])
x = 0.18361848856646254
y = -4.2881577071112806
theta = -2.2011317013010205
lx, ly, ltheta = lasers.car_loc_to_laser_loc(x, y, theta, a, b)
obs_lasers = fast.readings_for_obstacles(
lx, ly, ltheta, laser_angles, laser_max_range, obstacles)
# For debugging when the test fails:
if False:
for i, val in enumerate(obs_lasers):
print i, val
lasers.plot_lasers(
lx, ly, ltheta, laser_angles, laser_max_range,
obstacles, obs_lasers, plt.gca())
plt.show()
nose.tools.assert_almost_equals(obs_lasers[16], 1.89867472652, 10)
nose.tools.assert_almost_equals(obs_lasers[85], 10.0, 10)
nose.tools.assert_almost_equals(obs_lasers[278], 0.739595449593, 10)
def advance_particle(self, particle):
old_state = [particle.x, particle.y, particle.theta]
velocity = self.current_velocity
steering = self.current_steering
# Add controls noise:
if self.add_controls_noise:
velocity += np.random.normal(
loc=0.0, scale=self.velocity_noise_stdev)
steering += np.random.normal(
loc=0.0, scale=self.steering_noise_stdev)
steering = normalize_radians(steering)
# Dynamics update:
prev_timestamp = 0.0
if self.current_ts > 0:
prev_timestamp = self.dataset.timestamps[self.current_ts - 1]
delta_t = self.dataset.timestamps[self.current_ts] - prev_timestamp
assert delta_t > 0
new_state = draw_dr.dynamics(
self.dataset, old_state, velocity, steering, delta_t)
# Add dynamics noise:
if self.add_dynamics_noise:
new_state[0] += np.random.normal(
loc=0.0, scale=self.x_noise_stdev)
new_state[1] += np.random.normal(
loc=0.0, scale=self.y_noise_stdev)
new_state[2] += np.random.normal(
loc=0.0, scale=self.theta_noise_stdev)
new_state[2] = normalize_radians(new_state[2])
# Extract obstacles.
obstacles = particle.obstacles
if (len(obstacles) == 0 and
self.dataset.ts2sensor[self.current_ts] == 'laser'):
laser_x, laser_y, laser_theta = lasers.car_loc_to_laser_loc(
particle.x, particle.y, particle.theta,
self.dataset.a, self.dataset.b)
obstacles = ransac.extract_obstacles(
laser_x, laser_y, laser_theta,
self.laser_angles, self.laser_max_range,
np.array(self.dataset.ts2laser[self.current_ts]))
# print "old obstacles:", particle.obstacles
# print "new obstacles:", obstacles
return LocPFParticle(
new_state[0], new_state[1], new_state[2], obstacles)
def logweigh_particle(self, particle):
logweight = 0.0
if self.dataset.ts2sensor[self.current_ts] == 'laser':
laser_x, laser_y, laser_theta = lasers.car_loc_to_laser_loc(
particle.x, particle.y, particle.theta,
self.dataset.a, self.dataset.b)
true_lasers = readings_for_obstacles(
laser_x, laser_y, laser_theta,
self.laser_angles, self.laser_max_range,
particle.obstacles)
obs_lasers = np.array(self.dataset.ts2laser[self.current_ts])
logweight = norm_log_pdf_id_cov(
obs_lasers, true_lasers, self.obs_cov_scale)
return logweight
def draw_map(ax, dataset):
"""
Draw map based on laser, intensity, and GPS data.
"""
assert LASER_COLS == INTENSITY_COLS == 361
obst_xs = []
obst_ys = []
obst_cs = []
agent_xs = []
agent_ys = []
prev_gps_reading = None
for ts, time in enumerate(dataset.timestamps):
if dataset.ts2sensor[ts] == 'gps':
prev_gps_reading = dataset.ground_ts2gps[ts]
if dataset.ts2sensor[ts] == 'laser':
agent_x, agent_y, agent_phi = prev_gps_reading
laser_x, laser_y, _ = car_loc_to_laser_loc(
agent_x, agent_y, agent_phi, dataset.a, dataset.b)
agent_xs.append(agent_x)
agent_ys.append(agent_y)
for i in xrange(LASER_COLS):
distance = dataset.ts2laser[ts][i]
if 0.9 < distance / LASER_MAX < 1.1:
continue # no obstacles within laser range
intensity = dataset.ts2intensity[ts][i]
radians = agent_phi + (-90 + 0.5 * i) * np.pi / 180
obst_x = laser_x + distance * np.cos(radians)
obst_y = laser_y + distance * np.sin(radians)
obst_c = intensity / INTENSITY_MAX
obst_xs.append(obst_x)
obst_ys.append(obst_y)
obst_cs.append(obst_c)
print "Have {} agent locations and {} obstacle locations.".format(
len(agent_xs), len(obst_xs))
ax.scatter(agent_xs, agent_ys, c='red', linewidths=0)
# ax.scatter(obst_xs, obst_ys, c=obst_cs, linewidths=0, alpha=0.01)
ax.scatter(obst_xs, obst_ys, c='blue', linewidths=0, alpha=0.01)
true_obst_xs = []
true_obst_ys = []
if dataset.ground_obstacles:
true_obst_xs, true_obst_ys = zip(*dataset.ground_obstacles)
ax.scatter(true_obst_xs, true_obst_ys, c='yellow')
ax.set_xlim(X_MIN - 1, X_MAX + 1)
ax.set_ylim(Y_MIN - 1, Y_MAX + 1)
plt.draw()
from ppaml_car import data
from ppaml_car import lasers
from ppaml_car import ransac
import numpy as np
def extract_obstacles_py():
ransac.extract_obstacles(
lx, ly, lt, laser_angles, laser_max_range, obs_lasers)
if __name__ == "__main__":
dataset = data.Dataset.read("2_bend")
cx, cy, ct = dataset.init_x, dataset.init_y, dataset.init_angle
lx, ly, lt = lasers.car_loc_to_laser_loc(cx, cy, ct, dataset.a, dataset.b)
obs_lasers = None
for ts in xrange(len(dataset.timestamps)):
if dataset.ts2sensor[ts] == 'laser':
obs_lasers = np.array(dataset.ts2laser[ts])
break
assert obs_lasers is not None
laser_angles = lasers.default_laser_angles()
laser_max_range = lasers.default_laser_max_range()
import time
timer = time.time()
num_trials = 1000
for t in xrange(num_trials):
extract_obstacles_py()
timer = time.time() - timer
# theta = -0.000112593
# LW MAP, state noise 0.01, laser noise 0.01, 100K samples, trial 1:
x = -6.111071374818211
y = -0.10284180236291353
theta = 0.013397688774164038
# LW MAP, state noise 0.01, laser noise 0.01, 100K samples, trial 2:
x = -6.111312885927913
y = -0.11534201599817488
theta = 0.01650033592056109
# Compute laser location from vehicle (rear-axle) location:
a = 0.299541
b = 0.0500507
laser_x, laser_y, laser_theta = car_loc_to_laser_loc(x, y, theta, a, b)
laser_angles = np.arange(-90, 90.5, 0.5) * np.pi / 180
laser_max_range = 10
obstacles = np.array([
[-4.475, 1.45, 0.4],
[-1.3, 1.025, 0.4],
[-3.0, -1.55, 0.4],
[0.65, -1.95, 0.4],
[-1.95, -3.8, 0.4],
[0.15, -5.625, 0.4]
])
plt.figure(figsize=(8, 8))
plot_lasers(
laser_x, laser_y, laser_theta, laser_angles, laser_max_range,
if __name__ == "__main__":
min_x = data.X_MIN
max_x = data.X_MAX
min_y = data.Y_MIN
max_y = data.Y_MAX
min_theta = -np.pi
max_theta = np.pi
a = 0.299541
b = 0.0500507
# Location from ground GPS:
true_x = -6.1039
true_y = -0.0499926
true_theta = -0.000112593
true_x, true_y, true_theta = lasers.car_loc_to_laser_loc(
true_x, true_y, true_theta, a, b)
# Location from noisy GPS:
fixed_x = -6.09833
fixed_y = -0.0897621
fixed_theta = -0.000112593
fixed_x, fixed_y, fixed_theta = lasers.car_loc_to_laser_loc(
fixed_x, fixed_y, fixed_theta, a, b)
# Location from LW:
fixed_x = -6.111071374818211
fixed_y = -0.10284180236291353
fixed_theta = 0.013397688774164038
fixed_x, fixed_y, fixed_theta = lasers.car_loc_to_laser_loc(
fixed_x, fixed_y, fixed_theta, a, b)
