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