def draw_fake_balloon(self, frame, veh_pos, balloon_pos, vehicle_roll,
vehicle_pitch, vehicle_yaw):
# calculate bearing to balloon
bearing_to_balloon = PositionVector.get_bearing(veh_pos, balloon_pos)
yaw_to_balloon = balloon_utils.wrap_PI(bearing_to_balloon -
vehicle_yaw)
# calculate earth frame pitch angle from vehicle to balloon
pitch_to_balloon = vehicle_pitch + PositionVector.get_elevation(
veh_pos, balloon_pos)
#print "Fake Balloon Bearing:%f Pitch:%f Dist:%f" % (math.degrees(bearing_to_balloon), math.degrees(pitch_to_balloon), dist_to_balloon_xy)
# calculate pixel position of balloon
balloon_x = balloon_video.angle_to_pixels_x(
yaw_to_balloon) + balloon_video.img_center_x
balloon_y = balloon_video.angle_to_pixels_y(
pitch_to_balloon) + balloon_video.img_center_y
# calculate size of balloon in pixels from distance and size
dist_to_balloon_xyz = PositionVector.get_distance_xyz(
veh_pos, balloon_pos)
balloon_radius = balloon_utils.get_pixels_from_distance(
dist_to_balloon_xyz, balloon_finder.balloon_radius_expected)
# store balloon radius
self.last_balloon_radius = balloon_radius
# draw balloon
cv2.circle(frame, (balloon_x, balloon_y), balloon_radius,
self.fake_balloon_colour_bgr_scalar, -1)
def draw_fake_balloon(self, frame, veh_pos, balloon_pos, vehicle_roll, vehicle_pitch, vehicle_yaw):
# calculate bearing to balloon
bearing_to_balloon = PositionVector.get_bearing(veh_pos, balloon_pos)
yaw_to_balloon = balloon_utils.wrap_PI(bearing_to_balloon-vehicle_yaw)
# calculate earth frame pitch angle from vehicle to balloon
pitch_to_balloon = vehicle_pitch + PositionVector.get_elevation(veh_pos, balloon_pos)
#print "Fake Balloon Bearing:%f Pitch:%f Dist:%f" % (math.degrees(bearing_to_balloon), math.degrees(pitch_to_balloon), dist_to_balloon_xy)
# calculate pixel position of balloon
balloon_x = balloon_video.angle_to_pixels_x(yaw_to_balloon) + balloon_video.img_center_x
balloon_y = balloon_video.angle_to_pixels_y(pitch_to_balloon) + balloon_video.img_center_y
# calculate size of balloon in pixels from distance and size
dist_to_balloon_xyz = PositionVector.get_distance_xyz(veh_pos, balloon_pos)
balloon_radius = balloon_utils.get_pixels_from_distance(dist_to_balloon_xyz, balloon_finder.balloon_radius_expected)
# store balloon radius
self.last_balloon_radius = balloon_radius
# draw balloon
cv2.circle(frame,(balloon_x,balloon_y), balloon_radius, self.fake_balloon_colour_bgr_scalar, -1)
def get_pixels_from_distance(distance, actual_size):
return balloon_video.angle_to_pixels_x(actual_size / distance)
def get_pixels_from_distance(distance, actual_size):
return balloon_video.angle_to_pixels_x(actual_size / distance)