def get_lane_poses(dw, q, tol=0.000001):
from duckietown_world.geo.measurements_utils import iterate_by_class, IterateByTestResult
from .lane_segment import LaneSegment
from duckietown_world import TileCoords
for it in iterate_by_class(dw, Tile):
assert isinstance(it, IterateByTestResult), it
assert isinstance(it.object, Tile), it.object
tile = it.object
tile_fqn = it.fqn
tile_transform = it.transform_sequence
for _ in tile_transform.transforms:
if isinstance(_, TileCoords):
tile_coords = _
break
else:
msg = 'Could not find tile coords in %s' % tile_transform
assert False, msg
# print('tile_transform: %s' % tile_transform.asmatrix2d().m)
tile_relative_pose = relative_pose(tile_transform.asmatrix2d().m, q)
p = translation_from_O3(tile_relative_pose)
# print('tile_relative_pose: %s' % tile_relative_pose)
if not tile.get_footprint().contains(p):
continue
nresults = 0
for it2 in iterate_by_class(tile, LaneSegment):
lane_segment = it2.object
lane_segment_fqn = tile_fqn + it2.fqn
assert isinstance(lane_segment, LaneSegment), lane_segment
lane_segment_wrt_tile = it2.transform_sequence.asmatrix2d()
lane_segment_relative_pose = relative_pose(lane_segment_wrt_tile.m,
tile_relative_pose)
lane_segment_transform = TransformSequence(
tile_transform.transforms + it2.transform_sequence.transforms)
lane_pose = lane_segment.lane_pose_from_SE2(
lane_segment_relative_pose, tol=tol)
M = lane_segment_transform.asmatrix2d().m
center_point = lane_pose.center_point.as_SE2()
center_point_abs = np.dot(M, center_point)
center_point_abs_t = Matrix2D(center_point_abs)
if lane_pose.along_inside and lane_pose.inside and lane_pose.correct_direction:
yield GetLanePoseResult(
tile=tile,
tile_fqn=tile_fqn,
tile_transform=tile_transform,
tile_relative_pose=Matrix2D(tile_relative_pose),
lane_segment=lane_segment,
lane_segment_relative_pose=Matrix2D(
lane_segment_relative_pose),
lane_pose=lane_pose,
lane_segment_fqn=lane_segment_fqn,
lane_segment_transform=lane_segment_transform,
tile_coords=tile_coords,
center_point=center_point_abs_t)
nresults += 1
def asmatrix2d(self):
angle0 = {"N": 0, "E": -np.pi / 2, "S": np.pi, "W": +np.pi / 2}[self.orientation] + np.pi / 2
angle = {"N": np.pi / 2, "E": 0, "S": np.pi + np.pi / 2, "W": np.pi}[self.orientation]
assert np.allclose(angle, angle0)
p = [self.i + 0.5, self.j + 0.5]
M = geometry.SE2_from_translation_angle(p, angle)
return Matrix2D(M)
def asmatrix2d(self):
# orientation2deg = dict(N=0, E=90, S=180, W=270, )
# angle = ['S', 'E', 'N', 'W'].index(self.orientation)
# angle = +angle * np.pi/2 + np.pi
angle = {
'N': 0,
'E': -np.pi/2,
'S': np.pi,
'W': +np.pi/2,
}[self.orientation] + np.pi/2
# angle = 0
# orientation2deg = dict(N=0, E=-90, S=-180, W=-270, )
# angle = np.deg2rad(orientation2deg[self.orientation])
p = [self.i + 0.5, self.j + 0.5]
M = geometry.SE2_from_translation_angle(p, angle)
return Matrix2D(M)
def asmatrix2d(self):
angle0 = {
'N': 0,
'E': -np.pi / 2,
'S': np.pi,
'W': +np.pi / 2,
}[self.orientation] + np.pi / 2
angle = {
'N': np.pi / 2,
'E': 0,
'S': np.pi + np.pi / 2,
'W': np.pi,
}[self.orientation]
assert np.allclose(angle, angle0)
p = [self.i + 0.5, self.j + 0.5]
M = geometry.SE2_from_translation_angle(p, angle)
return Matrix2D(M)
def asmatrix2d(self):
M = geometry.SE2_from_translation_angle(self.p, self.theta)
return Matrix2D(M)