def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(
self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll,
veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(
xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch,
veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(
size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(
self.vehicle_pos, self.balloon_pitch, self.balloon_heading,
self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(
LocationGlobal(-35.362938, 149.165085, 0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(
self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0, 0, fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(
veh_pos, fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while (True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch,
veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(
img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(
veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (
dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll, veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch, veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(self.vehicle_pos, self.balloon_pitch, self.balloon_heading, self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(Location(-35.362938,149.165085,0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0,0,fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(veh_pos,fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while(True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()