def get_estimate(t, n, t_est, n_est):
""" Returns xy, theta """
# find theta that makes n and n_est rotate
alpha1 = np.arctan2(n[1], n[0])
alpha2 = np.arctan2(n_est[1], n_est[0])
theta = dtu.norm_angle(alpha2 - alpha1)
R = SO2_from_angle(theta)
double_check = False
if double_check:
# print('alpha %s %s theta: %s' % (np.rad2deg(alpha1),
# np.rad2deg(alpha2),
# np.rad2deg(theta)))
assert_almost_equal(np.dot(R, n[:2]), n_est[:2])
t = t[0:2]
t_est = t_est[0:2]
xy = t - np.dot(R.T, t_est)
# verify
# xy + R(theta) t_est == t
if double_check:
# print('t %s t_est %s xy %s' % (t, t_est, xy))
assert_almost_equal(xy + np.dot(R.T, t_est), t)
return xy, -theta
def pose_from_coords(self, res):
self._init_metrics()
x = self.default_x
y = res['d'] - self.offset
theta = dtu.norm_angle(res['phi'])
pose = dtu.geo.SE2_from_translation_angle([x,y], theta)
return pose
def coords_from_pose(self, pose):
self._init_metrics()
xy, theta = dtu.geo.translation_angle_from_SE2(pose)
res = np.zeros((), dtype=self.dt)
# x is unused (projection)
res['phi'] = dtu.norm_angle(theta)
res['d'] = xy[1] + self.offset
return res
def generate_measurement_likelihood(self, segments):
# initialize measurement likelihood to all zeros
measurement_likelihood = self.grid_helper.create_new('float32')
measurement_likelihood.fill(0)
hit = miss = 0
pose_weight = []
num_by_color = defaultdict(lambda: 0)
adjust_by_number = False # add to configuration
with dtu.timeit_clock("pose gen for %s segs" % len(segments)):
for segment in segments:
num_by_color[segment.color] += 1
for segment in segments:
for pose, weight in self.generate_votes(
segment, self.delta_segment):
if self.bounds_theta_deg is not None:
theta_deg = np.rad2deg(
dtu.norm_angle(dtu.geo.angle_from_SE2(pose)))
m, M = self.bounds_theta_deg
if not (m <= theta_deg < M):
continue
if adjust_by_number:
n = num_by_color[segment.color]
weight_adjusted = weight * 1.0 / n
else:
weight_adjusted = weight
pose_weight.append((pose, weight_adjusted))
values = []
with dtu.timeit_clock("generating coords for %s votes" %
len(pose_weight)):
for pose, weight in pose_weight:
est = self._localization_template.coords_from_pose(pose)
value = dict((k, float(est[k])) for k in self.variables)
values.append((value, weight))
with dtu.timeit_clock("add voting for %s votes" % len(pose_weight)):
for value, weight in values:
# if value['dstop'] > 0.3:
# print('value:%s' % value)
# else:
# continue
added = self.grid_helper.add_vote(measurement_likelihood,
value,
weight,
F=self.F)
hit += added > 0
miss += added == 0
dtu.logger.debug('hit: %s miss : %s' % (hit, miss))
if np.linalg.norm(measurement_likelihood) == 0:
return None
return measurement_likelihood