def compare_faster():
variables = collections.OrderedDict()
variables['alpha'] = dict(min=-180,
max=180,
description="angle",
resolution=5,
units='deg',
units_display='deg')
variables['r'] = dict(min=3,
max=5,
description="distance",
resolution=0.1,
units='m',
units_display='cm')
# this will fail if precision is float32
gh = GridHelper(variables, precision='float64')
val_fast = gh.create_new()
val_fast.fill(0)
val_slow = gh.create_new()
val_slow.fill(0)
od = dtu.get_output_dir_for_test()
F = 1
alpha0 = 7
# r0 = 4
r0 = 4.1
w0 = 1.
value = dict(alpha=alpha0, r=r0)
gh.add_vote(val_slow, value, w0, F)
assert_equal(np.sum(val_slow > 0), 9)
values = np.zeros((2, 1))
values[0, 0] = alpha0
values[1, 0] = r0
weights = np.zeros(1)
weights[0] = w0
gh.add_vote_faster(val_fast, values, weights, F)
assert_equal(np.sum(val_fast > 0), 9)
d = grid_helper_plot(gh, val_slow)
fn = os.path.join(od, 'compare_faster_slow.jpg')
dtu.write_data_to_file(d.get_png(), fn)
d = grid_helper_plot(gh, val_fast)
fn = os.path.join(od, 'compare_faster_fast.jpg')
dtu.write_data_to_file(d.get_png(), fn)
D = val_fast - val_slow
diff = np.max(np.abs(D))
print('diff: %r' % diff)
if diff > 1e-8:
print(dtu.indent(array_as_string_sign(val_fast), 'val_fast '))
print(dtu.indent(array_as_string_sign(val_slow), 'val_slow '))
print(dtu.indent(array_as_string_sign(D), 'Diff '))
print('non zero val_fast: %s' % val_fast[val_fast > 0])
print('non zero val_slow: %s' % val_slow[val_slow > 0])
assert_almost_equal(val_fast, val_slow)
def generate_measurement_likelihood_faster(self, segments: List[Segment]):
with dtu.timeit_clock(f"get_compat_representation_obs ({len(segments)} segments)"):
rep_obs = get_compat_representation_obs(segments)
rep_map = self.rep_map
with dtu.timeit_clock(
f"generate_votes_faster (map: {len(rep_map.weight)}, obs: {len(rep_obs.weight)})"
):
votes = generate_votes_faster(rep_map, rep_obs)
if self.bounds_theta_deg is not None:
weight = votes.weight.copy()
theta_min = np.deg2rad(self.bounds_theta_deg[0])
theta_max = np.deg2rad(self.bounds_theta_deg[1])
num_outside = 0
for i in range(weight.shape[0]):
theta = votes.theta[i]
if not (theta_min <= theta <= theta_max):
weight[i] = 0
num_outside += 1
# print('Removed %d of %d because outside box' % (num_outside, len(weight)))
votes = remove_zero_weight(votes._replace(weight=weight))
with dtu.timeit_clock(f"compute pos iterative ({len(votes.weight)})"):
locations = self._localization_template.coords_from_position_orientation(votes.p, votes.theta)
num = len(locations)
est = np.zeros((2, num))
for i in range(2):
v = list(self.variables)[i]
est[i, :] = locations[v]
F = self.F
compare = False
with dtu.timeit_clock(f"add voting faster ({len(votes.weight)})"):
measurement_likelihood = self.grid_helper.create_new("float32")
measurement_likelihood.fill(0)
if compare:
counts1 = np.zeros(measurement_likelihood.shape, dtype="int")
else:
counts1 = None
added = self.grid_helper.add_vote_faster(
measurement_likelihood, est, votes.weight, F=F, counts=counts1
)
# print('Counts (new):\n' + array_as_string(counts1, lambda x: ' %3d' % x))
if compare:
with dtu.timeit_clock("add voting (traditional)"):
measurement_likelihood_classic = self.grid_helper.create_new("float32")
measurement_likelihood_classic.fill(0)
hit = miss = 0
counts2 = np.zeros(measurement_likelihood.shape, dtype="int")
for i in range(len(locations)):
loc = locations[i]
weight = votes.weight[i]
value = dict((k, loc[k]) for k in self.variables)
added = self.grid_helper.add_vote(
measurement_likelihood_classic, value, weight, F=F, counts=counts2
)
hit += added > 0
miss += added == 0
# dtu.logger.debug('tradoitional hit: %s miss : %s' % (hit, miss))
# print('Counts (old):\n' + array_as_string(counts2, lambda x: ' %3d' % x))
#
diff = measurement_likelihood - measurement_likelihood_classic
deviation = np.max(np.abs(diff))
if deviation > 1e-6:
s = array_as_string_sign(diff)
print(f"max deviation: {deviation}")
print(s)
return measurement_likelihood
def compare_faster():
variables = {}
variables["alpha"] = dict(min=-180,
max=180,
description="angle",
resolution=5,
units="deg",
units_display="deg")
variables["r"] = dict(min=3,
max=5,
description="distance",
resolution=0.1,
units="m",
units_display="cm")
# this will fail if precision is float32
gh = GridHelper(variables, precision="float64")
val_fast = gh.create_new()
val_fast.fill(0)
val_slow = gh.create_new()
val_slow.fill(0)
od = dtu.get_output_dir_for_test()
F = 1
alpha0 = 7
# r0 = 4
r0 = 4.1
w0 = 1.0
value = dict(alpha=alpha0, r=r0)
gh.add_vote(val_slow, value, w0, F)
assert_equal(np.sum(val_slow > 0), 9)
values = np.zeros((2, 1))
values[0, 0] = alpha0
values[1, 0] = r0
weights = np.zeros(1)
weights[0] = w0
gh.add_vote_faster(val_fast, values, weights, F)
assert_equal(np.sum(val_fast > 0), 9)
d = grid_helper_plot(gh, val_slow)
fn = os.path.join(od, "compare_faster_slow.jpg")
dtu.write_data_to_file(d.get_png(), fn)
d = grid_helper_plot(gh, val_fast)
fn = os.path.join(od, "compare_faster_fast.jpg")
dtu.write_data_to_file(d.get_png(), fn)
D = val_fast - val_slow
diff = np.max(np.abs(D))
print(f"diff: {diff!r}")
if diff > 1e-8:
print(dtu.indent(array_as_string_sign(val_fast), "val_fast "))
print(dtu.indent(array_as_string_sign(val_slow), "val_slow "))
print(dtu.indent(array_as_string_sign(D), "Diff "))
print(f"non zero val_fast: {val_fast[val_fast > 0]}")
print(f"non zero val_slow: {val_slow[val_slow > 0]}")
assert_almost_equal(val_fast, val_slow)
