class MyTestCase(unittest.TestCase):
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
def test_plot(self):
self.sensor_model.plot_model()
def test_laser_state(self):
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 275)
self.assertEqual(y, 250)
self.assertEqual(theta, 0)
self.particle.theta = -math.pi / 2
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 250)
self.assertEqual(y, 225)
self.assertEqual(theta, -math.pi / 2)
def test_ray_cast(self):
laser_state = _laser_state(self.particle)
right = self.sensor_model._ray_cast(laser_state, -math.pi / 2)
self.assertAlmostEqual(right, 250, places=0) # should hit edge of map
straight = self.sensor_model._ray_cast(laser_state, 0)
self.assertAlmostEqual(
straight, 225,
places=0) # should hit wall in front, minus 25 for laser offset
left = self.sensor_model._ray_cast(laser_state, math.pi / 2)
self.assertAlmostEqual(left, 750,
places=0) # should hit edge of map on left
left_diagonal = self.sensor_model._ray_cast(laser_state, math.pi / 4)
self.assertAlmostEqual(left_diagonal,
225 / math.cos(math.pi / 4),
places=0) # should hit wall off to left
def test__get_probability(self):
log_probabilities = []
particle = Particle(map)
particle.x = 250
particle.y = 500
particle.theta = 0
for _ in range(0, 10):
x = 200 + 10 * _
ranges = []
for i in range(-90, 91):
side = abs(500.0 / (math.sin(i * math.pi / 180) + 0.000001))
wall = abs((500.0 - x - 25) /
(math.cos(i * math.pi / 180) + 0.0000001))
ranges.append(min(side, wall))
log_probabilities.append(
self.sensor_model.get_probability(particle, ranges))
most_likely_reading = max(enumerate(log_probabilities),
key=lambda x: x[1])[0]
self.assertEqual(most_likely_reading, 5)
class MyTestCase(unittest.TestCase):
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
def test_plot(self):
self.sensor_model.plot_model()
def test_laser_state(self):
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 275)
self.assertEqual(y, 250)
self.assertEqual(theta, 0)
self.particle.theta = -math.pi/2
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 250)
self.assertEqual(y, 225)
self.assertEqual(theta, -math.pi/2)
def test_ray_cast(self):
laser_state = _laser_state(self.particle)
right = self.sensor_model._ray_cast(laser_state, -math.pi/2)
self.assertAlmostEqual(right, 250, places=0) # should hit edge of map
straight = self.sensor_model._ray_cast(laser_state, 0)
self.assertAlmostEqual(straight, 225, places=0) # should hit wall in front, minus 25 for laser offset
left = self.sensor_model._ray_cast(laser_state, math.pi/2)
self.assertAlmostEqual(left, 750, places=0) # should hit edge of map on left
left_diagonal = self.sensor_model._ray_cast(laser_state, math.pi/4)
self.assertAlmostEqual(left_diagonal, 225/math.cos(math.pi/4), places=0) # should hit wall off to left
def test__get_probability(self):
log_probabilities = []
particle = Particle(map)
particle.x = 250
particle.y = 500
particle.theta = 0
for _ in range(0, 10):
x = 200 + 10*_
ranges = []
for i in range(-90, 91):
side = abs(500.0/(math.sin(i*math.pi/180)+0.000001))
wall = abs((500.0-x-25)/(math.cos(i*math.pi/180)+0.0000001))
ranges.append(min(side, wall))
log_probabilities.append(self.sensor_model.get_probability(particle, ranges))
most_likely_reading = max(enumerate(log_probabilities), key=lambda x: x[1])[0]
self.assertEqual(most_likely_reading, 5)
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