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: List[Segment]):
# 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(f"pose gen for {len(segments)} segs"):
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(f"generating coords for {len(pose_weight)} votes"):
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(f"add voting for {len(pose_weight)} votes"):
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(f"hit: {hit} miss : {miss}")
if np.linalg.norm(measurement_likelihood) == 0:
return None
return measurement_likelihood