def generate_votes(self, segment, delta):
"""
yields xytheta, weight
"""
p1 = np.array([segment.points[0].x, segment.points[0].y, segment.points[0].z])
p2 = np.array([segment.points[1].x, segment.points[1].y, segment.points[1].z])
p_hat = 0.5 * p1 + 0.5 * p2
d = np.linalg.norm(p1 - p2)
weight = d
n_hat = get_normal_outward_for_segment(p2, p1)
# SegmentsMap
sm = self._localization_template.get_map()
num = 0
for map_segment in sm.segments:
if map_segment.color == segment.color:
for p, n in iterate_segment_sections(sm, map_segment, delta):
xy, theta = get_estimate(p, n, p_hat, n_hat)
pose = SE2_from_translation_angle(xy, theta)
yield pose, weight
num += 1
if num == 0:
msg = f"No segment found for {segment.color}"
dtu.logger.debug(msg)
def get_compat_representation_obs(segments, precision="float32"):
num = len(segments)
C = np.empty(shape=num, dtype="uint8") # color
T = np.empty(shape=(2, num), dtype=precision) # position
N = np.empty(shape=(2, num), dtype=precision) # normal
W = np.empty(shape=num, dtype=precision) # weight
for i in range(num):
segment = segments[i]
points = segment.points
color = segment.color
p1 = np.array((points[0].x, points[0].y))
p2 = np.array((points[1].x, points[1].y))
p_hat = p1 * 0.5 + p2 * 0.5
diff = p1 - p2
distance = np.hypot(diff[0], diff[1])
weight = distance
C[i] = color
T[0, i] = p_hat[0]
T[1, i] = p_hat[1]
N[0, i] = +diff[1] / distance
N[1, i] = -diff[0] / distance
W[i] = weight
if False:
diffn = diff / distance
n_hat = np.array((diffn[1], -diffn[0], 0))
n_hat_slow = get_normal_outward_for_segment(p2, p1)
assert_almost_equal(n_hat, n_hat_slow)
assert segment.points[0].z == 0
assert segment.points[1].z == 0
C.flags.writeable = False
T.flags.writeable = False
N.flags.writeable = False
W.flags.writeable = False
return PNRep(t=T, color=C, n=N, weight=W)
def iterate_segment_sections(sm: SegmentsMap, map_segment: SegMapSegment, delta: float) -> Iterator:
""" Yields point, normal """
w1 = np.array(sm.points[map_segment.points[0]].coords)
w2 = np.array(sm.points[map_segment.points[1]].coords)
dist = np.linalg.norm(w1 - w2)
if dist == 0:
msg = f"Could not use degenerate segment (points: {w1} {w2}) "
raise ValueError(msg)
map_segment_n = get_normal_outward_for_segment(w1, w2)
# going from w1 to w2
dirv = (w2 - w1) / dist
n = int(np.ceil(dist / delta))
assert n >= 1
for i in range(n):
s = i + 0.5 # take middle of segment
p = w1 + dirv * delta * s
yield p, map_segment_n
