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)