def PlanAhead(self, nsteps, ntrajectories, niters): """ Simulate a bunch of random trajectories and return the best one. """ best_trajectory = [] best_entropy = float("inf") for ii in range(ntrajectories): current_pose = self.pose_ trajectory = [] # Choose a random trajectory. while len(trajectory) < nsteps: delta_x = np.random.random_integers(-1, 1) delta_y = np.random.random_integers(-1, 1) delta_angle = (self.angular_step_ * float(np.random.random_integers(-1, 1))) next_pose = GridPose2D.Copy(current_pose) if next_pose.MoveBy(delta_x, delta_y, delta_angle): trajectory.append(next_pose) current_pose = next_pose # Compute entropy. sensor = Sensor2D(self.sensor_params_, self.sources_) entropy = self.map_.SimulateTrajectory(sensor, trajectory, niters) # Compare to best. if entropy < best_entropy: best_trajectory = trajectory best_entropy = entropy # Return best. return best_trajectory
def __init__(self, num_rows, num_cols, num_sources, num_steps, angular_step, sensor_params, num_samples): """ Constructor. """ self.num_rows_ = num_rows self.num_cols_ = num_cols self.num_sources_ = num_sources self.num_steps_ = num_steps self.angular_step_ = angular_step self.sensor_params_ = sensor_params self.num_samples_ = num_samples # Keep track of map, pose, true sources, and past poses. self.map_ = GridMap2D(self.num_rows_, self.num_cols_, self.num_sources_) self.pose_ = GridPose2D(self.num_rows_, self.num_cols_, int(np.random.uniform(0.0, self.num_rows_)) + 0.5, int(np.random.uniform(0.0, self.num_cols_)) + 0.5, np.random.uniform(0.0, 2.0 * math.pi)) self.sources_ = [] self.past_poses_ = [] # Generate random sources on the grid. for ii in range(self.num_sources_): x = float(np.random.random_integers(0, self.num_rows_-1)) + 0.5 y = float(np.random.random_integers(0, self.num_cols_-1)) + 0.5 self.sources_.append(Source2D(x, y))
def DecodeTrajectory(delta_xs, delta_ys, delta_as, trajectory_id, initial_pose, num_steps): base = len(delta_xs) * len(delta_ys) * len(delta_as) trajectory = [] current_pose = initial_pose while trajectory_id > 0: remainder = trajectory_id % base # Convert remainder to delta tuple (dx, dy, da). x_id = remainder % len(delta_xs) y_id = ((remainder - x_id) / len(delta_xs)) % len(delta_ys) a_id = ((remainder - x_id - y_id * len(delta_xs)) / (len(delta_xs) * len(delta_ys))) dx = delta_xs[x_id] dy = delta_ys[y_id] da = delta_as[a_id] # Append to 'trajectory'. next_pose = GridPose2D.Copy(current_pose) assert next_pose.MoveBy(dx, dy, da) trajectory.append(next_pose) # Update 'trajectory_id'. trajectory_id = (trajectory_id - remainder) / base # Reset 'current_pose'. current_pose = next_pose # If not the right length, that means that the last remainders were 0. # Update 'trajectory' accordingly. while len(trajectory) < num_steps: next_pose = GridPose2D.Copy(current_pose) assert next_pose.MoveBy(delta_xs[0], delta_ys[0], delta_as[0]) trajectory.append(next_pose) current_pose = next_pose return trajectory
def __init__(self, nrows, ncols, k, angular_step, sensor_params): """ Constructor. Takes in dimensions, number of sources, resolution, and sensor parameters. """ self.angular_step_ = angular_step self.sensor_params_ = sensor_params self.map_ = GridMap2D(nrows, ncols, k) self.pose_ = GridPose2D(nrows, ncols, 0.5 * nrows, 0.5 * ncols, 0.0) self.sources_ = [] self.past_poses_ = [] # Generate random sources on the grid. for ii in range(k): x = math.floor(np.random.uniform(0.0, float(nrows))) + 0.5 y = math.floor(np.random.uniform(0.0, float(ncols))) + 0.5 self.sources_.append(Source2D(x, y))
def TakeStep(self, trajectory): """ Move one step along this trajectory. """ # Update list of past poses. current_pose = GridPose2D.Copy(self.pose_) self.past_poses_.append(current_pose) # Update pose. self.pose_ = trajectory[0] self.sensor_params_["x"] = self.pose_.x_ self.sensor_params_["y"] = self.pose_.y_ self.sensor_params_["angle"] = self.pose_.angle_ # Take scan, and update map. sensor = Sensor2D(self.sensor_params_, self.sources_) self.map_.Update(sensor) # Return entropy. return self.map_.Entropy()
def test_distribution_convergence(): # Max deviation at any point in estimated matrices/vectors. kPrecision = 0.5 # Define hyperparameters. kNumSamples = 1000 kNumRows = 5 kNumCols = 5 kNumSources = 1 kNumSteps = 1 kAngularStep = 0.25 * math.pi kSensorParams = { "x": kNumRows / 2, "y": kNumCols / 2, "angle": 0.0, "fov": 0.5 * math.pi } # Generate conditionals from the specified pose. pose = GridPose2D(kNumRows, kNumCols, int(np.random.uniform(0.0, kNumRows)) + 0.5, int(np.random.uniform(0.0, kNumCols)) + 0.5, np.random.uniform(0.0, 2.0 * math.pi)) # Create a problem. problem = Problem(kNumRows, kNumCols, kNumSources, kNumSteps, kAngularStep, kSensorParams, kNumSamples) (pzx1, hzm1, traj_ids1) = problem.GenerateConditionals(pose) (pzx2, hzm2, traj_ids2) = problem.GenerateConditionals(pose) # Check that distributions are close. assert_equal(traj_ids1, traj_ids2) assert_array_less(abs(pzx1 - pzx2).max(), kPrecision) assert_array_less(abs(hzm1 - hzm2).max(), kPrecision) # Check that cost vectors are not too different. c1 = pzx1.T * hzm1 c2 = pzx2.T * hzm2 assert_array_less(abs(c1 - c2).max(), kPrecision)
# Files to save to. pzx_file = "pzx_5x5_1000.csv" hzm_file = "hmz_5x5_1000.csv" # Define hyperparameters. kNumSamples = 1000 kNumRows = 5 kNumCols = 5 kNumSources = 1 kNumSteps = 1 kAngularStep = 0.25 * math.pi kSensorParams = {"x" : kNumRows/2, "y" : kNumCols/2, "angle" : 0.0, "fov" : 0.5 * math.pi} # Create a problem. problem = Problem(kNumRows, kNumCols, kNumSources, kNumSteps, kAngularStep, kSensorParams, kNumSamples) # Generate conditionals from the specified pose. pose = GridPose2D(kNumRows, kNumCols, kNumRows/2, kNumCols/2, 0.0) (pzx, hzm, trajectory_ids) = problem.GenerateConditionals(pose) print "P_{Z|X} shape: " + str(pzx.shape) print "h_{M|Z} shape: " + str(hzm.shape) np.savetxt(pzx_file, pzx, delimiter=",") np.savetxt(hzm_file, hzm, delimiter=",") print "Successfully saved to disk."
measurements = np.zeros((kNumSimulations, kNumSteps)) for ii in range(kNumSimulations): # Generate random sources on the grid. sources = [] for jj in range(kNumSources): x = float(np.random.random_integers(0, kNumRows - 1)) + 0.5 y = float(np.random.random_integers(0, kNumCols - 1)) + 0.5 sources.append(Source2D(x, y)) sensor = Sensor2D(kSensorParams, sources) maps[ii] = EncodeMap(kNumRows, kNumCols, sources) # Generate a valid trajectory of the given length. step_counter = 0 current_pose = GridPose2D(kNumRows, kNumCols, int(np.random.uniform(0.0, kNumRows)) + 0.5, int(np.random.uniform(0.0, kNumCols)) + 0.5, np.random.uniform(0.0, 2.0 * math.pi)) while step_counter < kNumSteps: dx = np.random.choice(delta_xs) dy = np.random.choice(delta_ys) da = np.random.choice(delta_as) next_pose = GridPose2D.Copy(current_pose) if next_pose.MoveBy(dx, dy, da): # If a valid move, append to list. trajectories[ii, step_counter] = ( int(next_pose.x_) + int(next_pose.y_) * kNumRows + (int(next_pose.angle_ / kAngularStep) % kNumAngles) * kNumRows * kNumAngles) current_pose = next_pose
def GenerateConditionals(self, pose): """ Generate conditional distribution matrix [P_{Z|X}] and conditional entropy vector [h_{M|Z}] by starting from the specified pose and generating a bunch of random legal trajectories, and for each one generating a random map and a measurement. The (i,j)-entry of [P_{Z|X}] is the normalized frequency of observing measurement i given that the trajectory chosen was j. This is computed by first estimating the joint distribution of Z and X and then normalizing each row so that it sums to unity. The i-entry of [h_{M|Z}] is the entropy of M given that we observed measurement i, starting from the given pose. This is computed by first estimating the joint distribution of M and Z, and then looking at the row where Z = i. """ # Set the choices for taking steps. delta_xs = [-1, 0, 1] delta_ys = [-1, 0, 1] delta_as = [-self.angular_step_, 0.0, self.angular_step_] # Compute the number of possible trajectories, maps, and measurements. kNumTrajectories = (len(delta_xs) * len(delta_ys) * len(delta_as))**self.num_steps_ kNumMaps = (self.num_rows_ * self.num_cols_)**self.num_sources_ kNumMeasurements = (self.num_sources_ + 1)**self.num_steps_ # Create a dictionary to hold the counts for each trajectory. zx_dict = {} # Create empty matrix to store the joint distribution [P_{M, Z}]. zm_joint = np.zeros((kNumMeasurements, kNumMaps)) # Generate a ton of sampled data. for ii in range(self.num_samples_): # Generate random sources on the grid according to 'map_prior' and # compute a corresponding map id number based on which grid cells # the sources lie in. sources = self.map_prior_.GenerateSources() map_id = EncodeMap(self.num_rows_, self.num_cols_, sources) # Create a sensor. sensor = Sensor2D(self.sensor_params_, sources) # Pick a random trajectory starting at the given pose. At each step, # get the corresponding measurement. current_pose = pose delta_sequence = [] measurements = [] while len(delta_sequence) < self.num_steps_: dx = np.random.choice(delta_xs) dy = np.random.choice(delta_ys) da = np.random.choice(delta_as) next_pose = GridPose2D.Copy(current_pose) if next_pose.MoveBy(dx, dy, da): # If a valid move, append to list. delta_sequence.append((dx, dy, da)) current_pose = next_pose # Get a measurement. sensor.ResetPose(current_pose) measurements.append(sensor.Sense()) # Get trajectory and measurement ids. trajectory_id = EncodeTrajectory(delta_xs, delta_ys, delta_as, delta_sequence) measurement_id = EncodeMeasurements(self.num_sources_, measurements) # Record this sample in the 'zm_joint' matrix. zm_joint[measurement_id, map_id] += 1.0 # Record this sample in the 'zx_dict' dictionary. if trajectory_id not in zx_dict: zx_dict[trajectory_id] = np.zeros(kNumMeasurements) zx_dict[trajectory_id][measurement_id] += 1.0 # Convert 'zx_dict' to a matrix. zx_joint = np.zeros((kNumMeasurements, len(zx_dict))) trajectory_ids = np.zeros(len(zx_dict), dtype=int) for ii, trajectory_id in enumerate(zx_dict.keys()): zx_joint[:, ii] = zx_dict[trajectory_id] trajectory_ids[ii] = trajectory_id # Normalize so that all the rows sum to unity. zx_conditional = zx_joint.copy() zm_conditional = zm_joint.copy() for ii in range(zx_joint.shape[0]): row_sum = zx_conditional[ii, :].sum() if row_sum < 1e-8: print "Encountered measurement with no support in P_{Z|X}." zx_conditional[ii, :] = 0.0 else: zx_conditional[ii, :] /= row_sum for ii in range(zm_joint.shape[0]): row_sum = zm_conditional[ii, :].sum() if row_sum < 1e-8: print "Encountered measurement with no support in P_{Z|M}." zm_conditional[ii, :] = 0.0 else: zm_conditional[ii, :] /= row_sum # zx_conditional = zx_joint / np.sum(zx_joint, axis=1)[:, None] # zm_conditional = zm_joint / np.sum(zm_joint, axis=1)[:, None] # Compute [h_{M|Z}], the conditional entropy vector. def entropy(distribution): return sum( map(lambda p: -max(p, 1e-4) * math.log(max(p, 1e-4)), distribution)) h_conditional = np.asarray( map( lambda measurement_id: entropy(zm_conditional[measurement_id, : ]), range(kNumMeasurements))) return (zx_conditional, h_conditional, trajectory_ids)