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 __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 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