def get_background(self, vehicle_roll, vehicle_pitch):
# create sky coloured image
image = numpy.zeros((balloon_video.img_height, balloon_video.img_width, 3),numpy.uint8)
image[:] = self.background_sky_colour_bgr
# create large rectangle which will become the ground
top_left = balloon_utils.rotate_xy(balloon_video.img_center_x-1000, balloon_video.img_center_y, -vehicle_roll)
top_right = balloon_utils.rotate_xy(balloon_video.img_center_x+1000, balloon_video.img_center_y, -vehicle_roll)
bot_left = balloon_utils.rotate_xy(balloon_video.img_center_x-1000,balloon_video.img_center_y+1000, -vehicle_roll)
bot_right = balloon_utils.rotate_xy(balloon_video.img_center_x+1000,balloon_video.img_center_y+1000, -vehicle_roll)
# calculate vertical pixel shift
pitch_pixel_shift = balloon_video.angle_to_pixels_y(vehicle_pitch)
# add pitch adjustment
top_left = balloon_utils.shift_pixels_down(top_left, pitch_pixel_shift)
top_right = balloon_utils.shift_pixels_down(top_right, pitch_pixel_shift)
bot_left = balloon_utils.shift_pixels_down(bot_left, pitch_pixel_shift)
bot_right = balloon_utils.shift_pixels_down(bot_right, pitch_pixel_shift)
# draw horizon
box = numpy.array([top_left, top_right, bot_right, bot_left],numpy.int32)
cv2.fillConvexPoly(image, box, self.background_ground_colour_bgr_scalar)
return image
def add_artificial_horizon(self, frame, vehicle_roll, vehicle_pitch):
# horizon line is 200 pixels in either direction from center of image
ah1_x, ah1_y = balloon_utils.rotate_xy(balloon_video.img_center_x - 200, balloon_video.img_center_y, -vehicle_roll)
ah2_x, ah2_y = balloon_utils.rotate_xy(balloon_video.img_center_x + 200, balloon_video.img_center_y, -vehicle_roll)
# shift down by by -ve pitch angle
pitch_pixel_shift = int(math.degrees(vehicle_pitch) / float(balloon_video.cam_vfov) * balloon_video.img_height)
# draw line
cv2.line(frame,(int(ah1_x),int(ah1_y)+pitch_pixel_shift),(int(ah2_x),int(ah2_y)+pitch_pixel_shift),5000,2)
def add_artificial_horizon(self, frame, vehicle_roll, vehicle_pitch):
# horizon line is 200 pixels in either direction from center of image
ah1_x, ah1_y = balloon_utils.rotate_xy(
balloon_video.img_center_x - 200, balloon_video.img_center_y,
-vehicle_roll)
ah2_x, ah2_y = balloon_utils.rotate_xy(
balloon_video.img_center_x + 200, balloon_video.img_center_y,
-vehicle_roll)
# shift down by by -ve pitch angle
pitch_pixel_shift = int(
math.degrees(vehicle_pitch) / float(balloon_video.cam_vfov) *
balloon_video.img_height)
# draw line
cv2.line(frame, (int(ah1_x), int(ah1_y) + pitch_pixel_shift),
(int(ah2_x), int(ah2_y) + pitch_pixel_shift), 5000, 2)
def pixels_to_direction(self, pixels_x, pixels_y, vehicle_roll, vehicle_pitch, vehicle_yaw):
# rotate position by +ve roll angle
x_rotated, y_rotated = balloon_utils.rotate_xy(pixels_x - balloon_video.img_width/2, pixels_y - balloon_video.img_height/2, vehicle_roll)
# calculate vertical pixel shift from pitch angle
pitch_pixel_shift = int(math.degrees(vehicle_pitch) / float(balloon_video.cam_vfov) * balloon_video.img_height)
pitch_dir = (-y_rotated + pitch_pixel_shift) / float(balloon_video.img_height) * balloon_video.cam_vfov
# calculate yaw shift in degrees
yaw_dir = x_rotated / float(balloon_video.img_width) * float(balloon_video.cam_hfov) + math.degrees(vehicle_yaw)
# return vertical angle to target and heading
return pitch_dir, yaw_dir
def pixels_to_direction(self, pixels_x, pixels_y, vehicle_roll,
vehicle_pitch, vehicle_yaw):
# rotate position by +ve roll angle
x_rotated, y_rotated = balloon_utils.rotate_xy(
pixels_x - balloon_video.img_width / 2,
pixels_y - balloon_video.img_height / 2, vehicle_roll)
# calculate vertical pixel shift from pitch angle
pitch_pixel_shift = int(
math.degrees(vehicle_pitch) / float(balloon_video.cam_vfov) *
balloon_video.img_height)
pitch_dir = (-y_rotated + pitch_pixel_shift) / float(
balloon_video.img_height) * balloon_video.cam_vfov
# calculate yaw shift in degrees
yaw_dir = x_rotated / float(balloon_video.img_width) * float(
balloon_video.cam_hfov) + math.degrees(vehicle_yaw)
# return vertical angle to target and heading
return pitch_dir, yaw_dir
