diff --git a/ch-13/4.3-monte-carlo_perf/display_arena.py b/ch-13/4.3-monte-carlo_perf/display_arena.py new file mode 100644 index 0000000..7eeec66 --- /dev/null +++ b/ch-13/4.3-monte-carlo_perf/display_arena.py @@ -0,0 +1,16 @@ +from matplotlib import pyplot as plt + +from robot import arena + +print(arena.distance_grid.min(), arena.distance_grid.max()) +for line in arena.boundary_lines: + plt.plot([line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black") +overscan_size = arena.overscan * arena.grid_cell_size +plt.imshow( + arena.distance_grid.T, + extent = [-overscan_size, arena.width + overscan_size, -overscan_size, arena.height + overscan_size], + origin="lower", + cmap="gray" +) + +plt.show() diff --git a/ch-13/4.3-monte-carlo_perf/robot/arena.py b/ch-13/4.3-monte-carlo_perf/robot/arena.py index 5425679..03735d9 100644 --- a/ch-13/4.3-monte-carlo_perf/robot/arena.py +++ b/ch-13/4.3-monte-carlo_perf/robot/arena.py @@ -9,6 +9,8 @@ height = 1500 cutout_width = 500 cutout_height = 500 +low_probability = 10 ** -10 + boundary_lines = [ [(0,0), (0, height)], [(0, height), (width, height)], @@ -65,7 +67,7 @@ def get_distance_likelihood(x, y): distance = get_distance_to_segment(x, y, segment) if min_distance is None or distance < min_distance: min_distance = distance - return 1.0 / (1 + min_distance/250) ** 2 + return 1.0 / (1 + min_distance/100) ** 2 # beam endpoint model @@ -88,10 +90,10 @@ def make_distance_grid(): distance_grid = make_distance_grid() -def get_distance_grid_at_point(x, y): +def get_distance_likelihood_at(x, y): """Return the distance grid value at the given point.""" grid_x = int(x // grid_cell_size + overscan) grid_y = int(y // grid_cell_size + overscan) if grid_x < 0 or grid_x >= distance_grid.shape[0] or grid_y < 0 or grid_y >= distance_grid.shape[1]: - return 0 + return low_probability return distance_grid[grid_x, grid_y] diff --git a/ch-13/4.3-monte-carlo_perf/robot/code.py b/ch-13/4.3-monte-carlo_perf/robot/code.py index eead9ff..6b73cae 100644 --- a/ch-13/4.3-monte-carlo_perf/robot/code.py +++ b/ch-13/4.3-monte-carlo_perf/robot/code.py @@ -79,16 +79,16 @@ class Simulation: int(random.uniform(0, arena.width)), int(random.uniform(0, arena.height)), int(random.uniform(0, 360))) for _ in range(self.population_size)], - dtype=np.float, + dtype=np.int16, ) self.distance_sensors = DistanceSensorTracker() self.collision_avoider = CollisionAvoid(self.distance_sensors) self.last_encoder_left = robot.left_encoder.read() self.last_encoder_right = robot.right_encoder.read() - self.alpha_rot = 0.05 - self.alpha_rot_trans = 0.01 - self.alpha_trans = 0.05 - self.alpha_trans_rot = 0.01 + self.alpha_rot = 0.09 + self.alpha_rot_trans = 0.05 + self.alpha_trans = 0.12 + self.alpha_trans_rot = 0.05 # profiling self.pc_resample = PerformanceCounter() @@ -99,26 +99,6 @@ class Simulation: self.pc_observe_distance_sensors = PerformanceCounter() self.pc_observation_model = PerformanceCounter() - def resample(self, weights, sample_count): - """Return sample_count number of samples from the - poses, based on the weights array. - Uses low variance resampling""" - self.pc_resample.start() - samples = np.zeros((sample_count, 3)) - interval = 1 / sample_count - shift = random.uniform(0, interval) - cumulative_weights = weights[0] - source_index = 0 - for current_index in range(sample_count): - weight_index = shift + current_index * interval - while weight_index >= cumulative_weights: - source_index += 1 - source_index = min(len(weights), source_index) - cumulative_weights += weights[source_index] - samples[current_index] = self.poses[source_index] - self.pc_resample.stop() - return samples - def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta): """ left_encoder is the change in the left encoder @@ -203,12 +183,14 @@ class Simulation: right_hypotenuse = np.sqrt(opposite**2 + adjacent**2) # modify the current weights based on the distance sensors - left_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float) + # left_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float) right_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float) # left sensor poses_left_angle = np.radians(self.poses[:, 2]) + left_angle - left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_angle) * left_hypotenuse - left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_angle) * left_hypotenuse + left_sensor = np.concatenate([ + self.poses[:, 0] + np.cos(poses_left_angle) * left_hypotenuse, + self.poses[:, 1] + np.sin(poses_left_angle) * left_hypotenuse + ], axis=1) # right sensor poses_right_angle = np.radians(self.poses[:, 2]) - right_angle @@ -217,8 +199,8 @@ class Simulation: # Look up the distance in the arena for index in range(self.poses.shape[0]): - sensor_weight = arena.get_distance_grid_at_point(left_sensor[index,0], left_sensor[index,1]) - sensor_weight += arena.get_distance_grid_at_point(right_sensor[index,0], right_sensor[index,1]) + sensor_weight = arena.get_distance_likelihood_at(left_sensor[index,0], left_sensor[index,1]) + sensor_weight += arena.get_distance_likelihood_at(right_sensor[index,0], right_sensor[index,1]) weights[index] *= sensor_weight self.pc_observe_distance_sensors.stop() return weights @@ -228,12 +210,38 @@ class Simulation: weights = np.ones(self.poses.shape[0], dtype=np.float) for index, pose in enumerate(self.poses): if not arena.contains(pose[:1], pose[:2]): - weights[index] = 0.01 + weights[index] = arena.low_probability weights = self.observe_distance_sensors(weights) - weights = weights / np.sum(weights) self.pc_observation_model.stop() return weights + def resample(self, weights, sample_count): + """Return sample_count number of samples from the + poses, based on the weights array. + Uses low variance resampling""" + self.pc_resample.start() + samples = np.zeros((sample_count, 3)) + interval = np.sum(weights) / sample_count + shift = random.uniform(0, interval) + cumulative_weights = weights[0] + source_index = 0 + try: + for current_index in range(sample_count): + weight_index = shift + current_index * interval + while weight_index >= cumulative_weights: + source_index += 1 + source_index = min(len(weights), source_index) + cumulative_weights += weights[source_index] + samples[current_index] = self.poses[source_index] + except IndexError: + send_json({"error": "IndexError in resample.", "weights": [weights.tolist()]}) + raise + if samples.shape[0] != sample_count: + send_json({"error": "Sample count mismatch in resample.", "samples": [samples.tolist()]}) + raise Exception("Sample count mismatch in resample.") + self.pc_resample.stop() + return samples + def print_pc_lines(self): if self.pc_odometry.count % 10 != 0: return diff --git a/ch-13/4.3-monte-carlo_perf/robot/performance_counter.py b/ch-13/4.3-monte-carlo_perf/robot/performance_counter.py index c8e2505..cef02fd 100644 --- a/ch-13/4.3-monte-carlo_perf/robot/performance_counter.py +++ b/ch-13/4.3-monte-carlo_perf/robot/performance_counter.py @@ -16,6 +16,8 @@ class PerformanceCounter: self.count += 1 def per_call(self): + if self.count == 0: + return 0 return self.total_time / self.count def total_call_time(self):