def test_straight_right(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] for _ in range(0, 100): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 100) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, 0)
def test_turn_right(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = 0 for _ in range(0, 10): odometry[2] -= math.pi/20 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 0) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, -math.pi/2)
def test_sidestep_right(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = math.pi/2 for _ in range(0, 100): odometry[1] -= 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 100) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, math.pi/2)
def test_straight_up(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 40 self.particle.y = 0 self.particle.theta = math.pi/2 for _ in range(0, 100): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 40) self.assertEqual(self.particle.y, 100) self.assertEqual(self.particle.theta, math.pi/2)
def test_distribution_line(self): motion_model = MotionModel(0.1, 0.1, 0.1, 0.1) odometry = [0, 0, 0] p, = plt.plot([], [], 'r.') for _ in range(0, 3): odometry[0] += 1 x = list() y = list() for particle in self.particles: motion_model.update(particle, odometry, 0) x.append(particle.x) y.append(particle.y) self.map.display(self.particles, title='Line w/ motion model')
def test_distribution_circle(self): motion_model = MotionModel(0.01, 0.01, 0.01, 0.01) odometry = [0, 0, 0] for particle in self.particles: particle.x = 0 particle.y = 40 particle.theta = math.pi/2 for _ in range(0, 4): odometry[0] += 10*math.sqrt(2)/2 odometry[1] -= 10*(1-math.sqrt(2)/2) odometry[2] -= math.pi/4 for particle in self.particles: motion_model.update(particle, odometry, 0) self.map.display(self.particles, title='Upper Circle w/ Motion Model')
def test_circle(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = math.pi/2 for _ in range(0, 4): odometry[0] += 10*math.sqrt(2)/2 odometry[1] -= 10*(1-math.sqrt(2)/2) odometry[2] -= math.pi/4 motion_model.update(self.particle, odometry, 0) #self.map.display([self.particle], title='Zero Variance Upper Circle') self.assertAlmostEqual(self.particle.x, 20) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, -math.pi/2)
def test_motion_model(): ### Generate test input np.random.seed(10000) np.set_printoptions(precision=4, suppress=True) map_obj = MapReader('../data/map/wean.dat') occupancy_map = map_obj.get_map() # # generate a random particle, then sent out beams from that location # h, w = occupancy_map.shape # indices = np.where(occupancy_map.flatten() == 0)[0] # ind = np.random.choice(indices, 1)[0] # y, x = ind // w, ind % w # theta = np.pi / 2 # X = np.array([[x, y, theta]]) # X[:, :2] *= 10 h, w = occupancy_map.shape flipped_occupancy_map = occupancy_map[::-1, :] y, x = np.where(flipped_occupancy_map == 0) valid_indices = np.where((350 <= y) & (y <= 450) & (350 <= x) & (x <= 450))[0] ind = indices = np.random.choice(valid_indices, 1)[0] y, x = y[ind], x[ind] # y, x = ind // w, ind % w theta = np.pi / 4 X = np.array([[x, y, theta]]) X[:, :2] *= 10 print(X) # define motion control u, test on shift-only theta1 = np.pi / 2 u_t1 = np.array([1049.833130, 28.126379, -2.664653]) u_t2 = np.array([1048.641235, 27.560303, -2.659024]) # motion model motion = MotionModel() X_t2 = motion.update(u_t1, u_t2, X) print(X_t2) print(f'X: {X}, X_t2: {X_t2}') # plot plt.figure(1) plt.imshow(occupancy_map, cmap='Greys') plt.axis([0, 800, 0, 800]) x_loc = X[:, 0] / 10. y_loc = X[:, 1] / 10. plt.scatter(x_loc, y_loc, c='r', marker='o') x_loc = X_t2[:, 0] / 10. y_loc = X_t2[:, 1] / 10. plt.scatter(x_loc, y_loc, c='y', marker='o') plt.show()
def test_ground_truth_motion(): # Read map np.random.seed(10000) np.set_printoptions(precision=4, suppress=True) map_obj = MapReader('../data/map/wean.dat') occupancy_map = map_obj.get_map() # specify ground truth location X = np.array([[4200, 4000, np.pi*(170/180.)]]) motion_model = MotionModel() with open('../data/log/robotdata1.log', 'r') as f: logfile = f.readlines() visualize_map(occupancy_map) visualize_timestep(X, 0, '../tmp') first_time_idx = True for time_idx, line in enumerate(logfile): meas_type = line[0] meas_vals = np.fromstring(line[2:], dtype=np.float64, sep=' ') odometry_robot = meas_vals[0:3] time_stamp = meas_vals[-1] if (meas_type == "L"): # [x, y, theta] coordinates of laser in odometry frame odometry_laser = meas_vals[3:6] # 180 range measurement values from single laser scan ranges = meas_vals[6:-1] # print("Processing time step {} at time {}s".format( # time_idx, time_stamp)) if first_time_idx: u_t0 = odometry_robot first_time_idx = False continue if meas_type == "O": continue u_t1 = odometry_robot # Update motion X = motion_model.update(u_t0, u_t1, X) u_t0 = u_t1 visualize_timestep(X, time_idx, '../tmp')
def test_forward_back(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = 0 for _ in range(0, 10): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) for _ in range(0, 10): odometry[2] += math.pi/10 motion_model.update(self.particle, odometry, 0) for _ in range(0, 10): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 0) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, math.pi)
first_time_idx = False continue X_bar_new = np.zeros((num_particles, 4), dtype=np.float64) u_t1 = odometry_robot # Note: this formulation is intuitive but not vectorized; looping in python is SLOW. # Vectorized version will receive a bonus. i.e., the functions take all particles as the input and process them in a vector. for m in range(0, num_particles): """ MOTION MODEL """ x_t0 = X_bar[m, 0:3] x_t1 = motion_model.update(u_t0, u_t1, x_t0) """ SENSOR MODEL """ if (meas_type == "L"): z_t = ranges w_t = sensor_model.beam_range_finder_model(z_t, x_t1, ray_map) X_bar_new[m, :] = np.hstack((x_t1, w_t)) else: X_bar_new[m, :] = np.hstack((x_t1, X_bar[m, 3])) X_bar = X_bar_new u_t0 = u_t1 """ RESAMPLING
# 180 range measurement values from single laser scan ranges = meas_vals[6:-1] print("Processing time step {} at time {}s".format( time_idx, time_stamp)) if first_time_idx: u_t0 = odometry_robot first_time_idx = False continue if meas_type == "L": u_t1 = odometry_robot # Update motion X_t1 = motion_model.update(u_t0, u_t1, X_bar[:, 0:3]) # Correction step z_t = ranges W_t = sensor_model.beam_range_finder_model(z_t, X_t1, num_beams) X_bar_new = np.hstack((X_t1, W_t[..., None])) u_t0 = u_t1 # Resampling step X_bar_new = resampler.low_variance_sampler(X_bar_new) # DBSCAN to sample subsets of particles if args.adaptive: X_bar_new = adaptively_choose_particle(X_bar_new) X_bar = X_bar_new
class ParticleFilter(object): """ This is the main particle filter object. """ def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = [] def log(self, file_handle): line = list() for particle in self.particles: loc = str(particle.x) + ',' + str(particle.y) line.append(loc) file_handle.write(';'.join(line)) file_handle.write('\n') def display(self): self.map.display(self.particles, ranges=self._ranges) def update(self, line): """ Update step. Reading is a single reading (i.e. line) from the log file Could be either an odometry or laser reading """ measurement_type = line[0] # O = odometry, L = laser scan measurements = np.fromstring(line[2:], sep=' ') odometry = measurements[0:3] time_stamp = measurements[-1] # last variable for particle in self.particles: self._motion_model.update(particle, odometry, time_stamp) if measurement_type == "L": odometry_laser = measurements[3:6] # coordinates of the laser in standard odometry frame self._ranges = measurements[6:-1] # exclude last variable, the time stamp self._particle_probabilities = list() # unnormalized sensor model probabilities of the particles for particle in self.particles: self._particle_probabilities.append(self._sensor_model.get_probability(particle, self._ranges)) argsorted_probabilities = np.argsort(-np.asarray(self._particle_probabilities)) self.particles[argsorted_probabilities[0]].debug = True self.particles[argsorted_probabilities[1]].debug = True self.particles[argsorted_probabilities[2]].debug = True def _resample(self): """ Resamples the particles given unnormalized particle probabilites """ particle_probabilities = np.asarray(self._particle_probabilities)/sum(self._particle_probabilities) # normalize indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True, p=particle_probabilities) indices.sort() previous_index = -1 new_particles = list() for index in indices: if index != previous_index: new_particles.append(self.particles[index]) else: new_particles.append(deepcopy(self.particles[index])) previous_index = index self.particles = new_particles
class ParticleFilter(object): """ This is the main particle filter object. """ def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = [] def log(self, file_handle): line = list() for particle in self.particles: loc = str(particle.x) + ',' + str(particle.y) line.append(loc) file_handle.write(';'.join(line)) file_handle.write('\n') def display(self): self.map.display(self.particles, ranges=self._ranges) def update(self, line): """ Update step. Reading is a single reading (i.e. line) from the log file Could be either an odometry or laser reading """ measurement_type = line[0] # O = odometry, L = laser scan measurements = np.fromstring(line[2:], sep=' ') odometry = measurements[0:3] time_stamp = measurements[-1] # last variable for particle in self.particles: self._motion_model.update(particle, odometry, time_stamp) if measurement_type == "L": odometry_laser = measurements[ 3:6] # coordinates of the laser in standard odometry frame self._ranges = measurements[ 6:-1] # exclude last variable, the time stamp self._particle_probabilities = list( ) # unnormalized sensor model probabilities of the particles for particle in self.particles: self._particle_probabilities.append( self._sensor_model.get_probability(particle, self._ranges)) argsorted_probabilities = np.argsort( -np.asarray(self._particle_probabilities)) self.particles[argsorted_probabilities[0]].debug = True self.particles[argsorted_probabilities[1]].debug = True self.particles[argsorted_probabilities[2]].debug = True def _resample(self): """ Resamples the particles given unnormalized particle probabilites """ particle_probabilities = np.asarray( self._particle_probabilities) / sum( self._particle_probabilities) # normalize indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True, p=particle_probabilities) indices.sort() previous_index = -1 new_particles = list() for index in indices: if index != previous_index: new_particles.append(self.particles[index]) else: new_particles.append(deepcopy(self.particles[index])) previous_index = index self.particles = new_particles