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)