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