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