def SE2Transform_from_lane_pose(self, lane_pose): beta = self.beta_from_along_lane(lane_pose.along_lane) # logger.info('beta: %s' % beta) center_point = self.center_point(beta) delta = np.array([0, lane_pose.lateral]) rel = geo.SE2_from_translation_angle(delta, lane_pose.relative_heading) # logger.info('rel: %s' % geo.SE2.friendly(rel)) res = geo.SE2.multiply(center_point, rel) return SE2Transform.from_SE2(res)
def lane_pose(self, along_lane: float, lateral: float, relative_heading: float) -> LanePose: beta = self.beta_from_along_lane(along_lane) center_point = self.center_point(beta) W2 = self.width / 2 lateral_inside = -W2 <= lateral <= W2 outside_right = lateral < -W2 outside_left = W2 < lateral distance_from_left = np.abs(+W2 - lateral) distance_from_right = np.abs(-W2 - lateral) distance_from_center = np.abs(lateral) L = self.get_lane_length() along_inside = 0 <= along_lane < L along_before = along_lane < 0 along_after = along_lane > L inside = lateral_inside and along_inside correct_direction = np.abs(relative_heading) <= np.pi / 2 return LanePose( inside=inside, lateral_inside=lateral_inside, outside_left=outside_left, outside_right=outside_right, distance_from_left=distance_from_left, distance_from_right=distance_from_right, relative_heading=relative_heading, along_inside=along_inside, along_before=along_before, along_after=along_after, along_lane=along_lane, lateral=lateral, lateral_left=W2, lateral_right=-W2, distance_from_center=distance_from_center, center_point=SE2Transform.from_SE2(center_point), correct_direction=correct_direction, )
def get_transform(desc, tm, tile_size): rotate_deg = desc.get('rotate', 0) rotate = np.deg2rad(rotate_deg) if 'pos' in desc: pos = desc['pos'] x = float(pos[0]) * tile_size # account for non-righthanded y = float(tm.W - pos[1]) * tile_size # account for non-righthanded rotate = -rotate transform = SE2Transform([x, y], rotate) return transform else: if 'pose' in desc: pose = Serializable.from_json_dict(desc['pose']) else: pose = SE2Transform.identity() if 'attach' in desc: attach = desc['attach'] tile_coords = tuple(attach['tile']) slot = str(attach['slot']) x, y = get_xy_slot(slot) i, j = tile_coords u, v = (x + i) * tile_size, (y + j) * tile_size transform = SE2Transform([u, v], rotate) q = geo.SE2.multiply(transform.as_SE2(), pose.as_SE2()) return SE2Transform.from_SE2(q) else: return pose
def transform_point(p): q = p.as_SE2() q2 = np.dot(M, q) p2 = SE2Transform.from_SE2(q2) return p2