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()