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