def predict_segments(sm: SegmentsMap, gpg: GroundProjectionGeometry) -> SegmentList:
"""
Predicts what segments the robot would see.
Assumes map is in FRAME_AXLE.
"""
x_frustum = +0.1
fov = np.deg2rad(150)
res = []
for segment in sm.segments:
p1 = segment.points[0]
p2 = segment.points[1]
# If we are both in FRAME_AXLE
if (sm.points[p1].id_frame != FRAME_AXLE) or (sm.points[p2].id_frame != FRAME_AXLE):
msg = "Cannot deal with points not in frame FRAME_AXLE"
raise NotImplementedError(msg)
w1 = sm.points[p1].coords
w2 = sm.points[p2].coords
coords_inside = clip_to_view([w1, w2], x_frustum, fov)
if not coords_inside:
continue
w1 = coords_inside[0]
w2 = coords_inside[1]
point1 = Point(w1[0], w1[1], w1[2])
point2 = Point(w2[0], w2[1], w2[2])
pixel1 = gpg.ground2pixel(GroundPoint(point1))
pixel2 = gpg.ground2pixel(GroundPoint(point2))
normalized1 = gpg.pixel2vector(pixel1)
normalized2 = gpg.pixel2vector(pixel2)
pixels_normalized = [normalized1, normalized2]
normal = Vector2D(0, 0) # XXX: compute normal
points = [point1, point2]
# color = segment_color_constant_from_string(segment.color)
assert segment.color in [Segment.RED, Segment.YELLOW, Segment.WHITE]
s = Segment(pixels_normalized=pixels_normalized, normal=normal, points=points, color=segment.color)
res.append(s)
return SegmentList(segments=res)
def rectify_segment(rectify: Rectify, gpg: GroundProjectionGeometry,
s1: Segment) -> Segment:
pixels_normalized = []
for i in (0, 1):
# normalized coordinates
nc = s1.pixels_normalized[i]
# get pixel coordinates
pixels = gpg.vector2pixel(nc)
# rectify
pr = rectify.rectify_point(pixels)
# recompute normalized coordinates
# t = Pixel(pr[0], pr[1])
v = gpg.pixel2vector(ImageSpaceResdepPoint(pr))
pixels_normalized.append(v)
s2 = Segment(color=s1.color, pixels_normalized=pixels_normalized)
return s2