def qr_tracking(droneVision:DroneVisionGUI, bebop:Bebop): cv2.namedWindow('qr') while cv2.getWindowProperty('qr', 0) >= 0: img = droneVision.get_latest_valid_picture() x,y,w,h = None,None,None,None try: rect = zbar.decode(img, symbols=[zbar.ZBarSymbol.QRCODE])[0][2] poly = zbar.decode(img, symbols=[zbar.ZBarSymbol.QRCODE])[0][3] x,y,w,h = rect p1,p2,p3,p4 = poly except IndexError: pass if x is not None: # cv2.rectangle(img,(x,y),(x+w,y+h),(0,0,255)) pts = np.array([[p1[0],p1[1]], [p2[0],p2[1]], [p3[0],p3[1]], [p4[0],p4[1]]], np.int32) pts.reshape(-1,1,2) cv2.polylines(img, [pts], True, (255, 0, 255), 5) area_t1 = abs((p1[0]*(p2[1]-p4[1])+p2[0]*(p4[1]-p1[1])+p4[0]*(p1[1]-p2[1]))/2.0) area_t2 = abs((p3[0] * (p2[1] - p4[1]) + p2[0] * (p4[1] - p3[1]) + p4[0] * (p3[1] - p2[1])) / 2.0) area = area_t2+area_t1 backup_threshold = 20000 fwd_threshold = 10000 if w is not None and area > backup_threshold: print("GOING BACK") bebop.fly_direct(roll=0, pitch=-20, yaw=0, vertical_movement=0, duration=0.07) elif w is not None and area < fwd_threshold: print("GOING FORWARD") bebop.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=0.07) if x is not None and x + (w / 2.0) > 550: print("GOING RIGHT") bebop.fly_direct(roll=0, pitch=0, yaw=100, vertical_movement=0, duration=0.1) elif x is not None and x + (w / 2.0) < 300: print("GOING LEFT") bebop.fly_direct(roll=0, pitch=0, yaw=-100, vertical_movement=0, duration=0.1) if x is not None and y + (h / 2.0) > 380: print("GOING DOWN") bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-40, duration=0.1) elif x is not None and y + (h / 2.0) < 100: print("GOING UP") bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=40, duration=0.1) cv2.imshow('qr', img) cv2.waitKey(10) bebop.safe_land(10) cv2.destroyAllWindows()
def face_tracking(droneVision:DroneVisionGUI,bebop:Bebop): face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml') cv2.namedWindow("face_tracking") frame = droneVision.get_latest_valid_picture() while cv2.getWindowProperty('face_tracking', 0) >= 0: frame = droneVision.get_latest_valid_picture() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, 1.3, 5) (x,y,w,h) = None, None, None, None if len(faces) > 0: (x,y,w,h) = faces[0] cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 255), 1) print("Face Size: "+ str(w*h)) backup_threshold = 3600 fwd_threshold = 2000 if w is not None and w*h > backup_threshold: print("GOING BACK") bebop.fly_direct(roll=0,pitch=-20,yaw=0,vertical_movement=0,duration=0.1) elif w is not None and w*h < fwd_threshold: print("GOING FORWARD") bebop.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=0.1) if x is not None and x+(w/2.0) > 650: print("GOING RIGHT") bebop.fly_direct(roll=0,pitch=0,yaw=70,vertical_movement=0,duration=0.1) elif x is not None and x+(w/2.0) < 200: print("GOING LEFT") bebop.fly_direct(roll=0, pitch=0, yaw=-70, vertical_movement=0, duration=0.1) cv2.imshow("face_tracking",frame) cv2.waitKey(10) bebop.safe_land(10) cv2.destroyAllWindows()
# disconnect nicely so we don't need a reboot print("disconnecting") bebop.disconnect() while True: bebop = Bebop() # connect to the bebop success = bebop.connect(5) if (success): # start up the video bebopVision = DroneVisionGUI( bebop, is_bebop=True, user_code_to_run=demo_user_code_after_vision_opened, user_args=(bebop, )) userVision = UserVision(bebopVision) bebopVision.set_user_callback_function( userVision.save_pictures, user_callback_args=None) # calls save picture continuously frame = bebopVision.get_latest_valid_picture() bebopVision.open_video() else: print("Error connecting to bebop. Retry")
def color_tracking(drone_vision:DroneVisionGUI, bebop:Bebop): def show_hist(hist): """Takes in the histogram, and displays it in the hist window.""" bin_count = hist.shape[0] bin_w = 24 img = np.zeros((256, bin_count * bin_w, 3), np.uint8) for i in range(bin_count): h = int(hist[i]) cv2.rectangle(img, (i * bin_w + 2, 255), ((i + 1) * bin_w - 2, 255 - h), (int(180.0 * i / bin_count), 255, 255), -1) img = cv2.cvtColor(img, cv2.COLOR_HSV2BGR) cv2.imshow('hist', img) showBackProj = False showHistMask = False frame = drone_vision.get_latest_valid_picture() if frame is not None: (hgt, wid, dep) = frame.shape cv2.namedWindow('camshift') cv2.namedWindow('hist') cv2.moveWindow('hist', 700, 100) # Move to reduce overlap # Initialize the track window to be the whole frame track_window = (0, 0, wid, hgt) # # Initialize the histogram from the stored image # Here I am faking a stored image with just a couple of blue colors in an array # you would want to read the image in from the file instead histImage = np.array([[[110, 70, 50]], [[111, 128, 128]], [[115, 100, 100]], [[117, 64, 50]], [[117, 200, 200]], [[118, 76, 100]], [[120, 101, 210]], [[121, 85, 70]], [[125, 129, 199]], [[128, 81, 78]], [[130, 183, 111]]], np.uint8) histImage = cv2.imread('orange.jpg') histImage = cv2.cvtColor(histImage,cv2.COLOR_BGR2HSV) maskedHistIm = cv2.inRange(histImage, np.array((0., 60., 32.)), np.array((180., 255., 255.))) cv2.imshow("masked",maskedHistIm) cv2.imshow("histim",histImage) hist = cv2.calcHist([histImage], [0], maskedHistIm, [16], [0, 180]) cv2.normalize(hist, hist, 0, 255, cv2.NORM_MINMAX) hist = hist.reshape(-1) show_hist(hist) # start processing frames while cv2.getWindowProperty('camshift', 0) >= 0: frame = drone_vision.get_latest_valid_picture() vis = frame.copy() hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # convert to HSV mask = cv2.inRange(hsv, np.array((0., 60., 32.)), np.array((180., 255., 255.))) # eliminate low and high saturation and value values # The next line shows which pixels are being used to make the histogram. # it sets to black all the ones that are masked away for being too over or under-saturated if showHistMask: vis[mask == 0] = 0 prob = cv2.calcBackProject([hsv], [0], hist, [0, 180], 1) prob &= mask term_crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1) track_box, track_window = cv2.CamShift(prob, track_window, term_crit) print(track_box[1][0]*track_box[1][1]) if showBackProj: vis[:] = prob[..., np.newaxis] try: cv2.ellipse(vis, track_box, (0, 0, 255), 2) area = track_box[1][0]*track_box[1][1] if area > 7000: print("GOING BACK") bebop.fly_direct(roll=0,pitch=-20,yaw=0,vertical_movement=0,duration=0.5) #bebop.smart_sleep(1) elif area < 4000: print("GOING FORWARD") bebop.fly_direct(roll=0,pitch=20,yaw=0,vertical_movement=0,duration=0.5) #bebop.smart_sleep(1) except: pass # print("Track box:", track_box) cv2.imshow('camshift', vis) ch = chr(0xFF & cv2.waitKey(5)) if ch == 'q': break elif ch == 'b': showBackProj = not showBackProj elif ch == 'v': showHistMask = not showHistMask bebop.safe_land(10) cv2.destroyAllWindows()
class DroneColorSegTest: def __init__(self, testFlying, mamboAddr, use_wifi): self.bb = [0, 0, 0, 0] self.bb_threshold = 4000 self.bb_trigger = False self.testFlying = testFlying self.mamboAddr = mamboAddr self.use_wifi = use_wifi self.mambo = Mambo(self.mamboAddr, self.use_wifi) self.mamboVision = DroneVisionGUI( self.mambo, is_bebop=False, buffer_size=200, user_code_to_run=self.mambo_fly_function, user_args=None) def color_segmentation(self, args): img = self.mamboVision.get_latest_valid_picture() if img is not None: [((x1, y1), (x2, y2)), ln_color] = cd_color_segmentation(img) self.bb = [x1, y1, x2, y2] bb_size = self.get_bb_size() print('bb_size:', bb_size) # cv2.imwrite('test_file.png', img) # uncomment to save latest pic if bb_size >= self.bb_threshold: print('orange detected') self.bb_trigger = True # else: # self.bb_trigger = False else: print('image is None') def get_bb_size(self): ''' returns area of self.bb (bounding box) ''' return (self.bb[2] - self.bb[0]) * (self.bb[3] - self.bb[1]) def mambo_fly_function(self, mamboVision, args): """ self.mambo takes off and yaws slowly in a circle until the vision processing detects a large patch of orange. It then performs a flip and lands. """ if (self.testFlying): print("taking off!") self.mambo.safe_takeoff(5) if (self.mambo.sensors.flying_state != "emergency"): print("flying state is %s" % self.mambo.sensors.flying_state) print("Flying direct: going up") self.mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=15, duration=2) print("flying state is %s" % self.mambo.sensors.flying_state) print("yawing slowly") for deg in range(36): self.mambo.turn_degrees(10) if self.bb_trigger: break self.mambo.smart_sleep(1) print("flying state is %s" % self.mambo.sensors.flying_state) print("flip left") success = self.mambo.flip(direction="left") print("self.mambo flip result %s" % success) self.mambo.smart_sleep(2) print("landing") print("flying state is %s" % self.mambo.sensors.flying_state) self.mambo.safe_land(5) else: print("Sleeeping for 15 seconds - move the self.mambo around") self.mambo.smart_sleep(15) # done doing vision demo print("Ending the sleep and vision") self.mamboVision.close_video() self.mambo.smart_sleep(5) print("disconnecting") self.mambo.disconnect() def run_test(self): print("trying to connect to self.mambo now") success = self.mambo.connect(num_retries=3) print("connected: %s" % success) if (success): # get the state information print("sleeping") self.mambo.smart_sleep(1) self.mambo.ask_for_state_update() self.mambo.smart_sleep(1) print("Preparing to open vision") self.mamboVision.set_user_callback_function( self.color_segmentation, user_callback_args=None) self.mamboVision.open_video()