#! /usr/bin/python3 from picamera.array import PiRGBArray from picamera import PiCamera from datetime import datetime import time import cv2 import sys import imutils import np import math # for sqrt distance formula # i2c stuff import smbus bus = smbus.SMBus(1) SLAVE_ADDRESS = 0x04 # Create the haar cascade #frontalfaceCascade = cv2.CascadeClassifier("haarcascade_frontalface_default.xml") frontalfaceCascade = cv2.CascadeClassifier("frontalface_fromweb.xml") profilefaceCascade = cv2.CascadeClassifier("haarcascade_profileface.xml") face = [0,0,0,0] # This will hold the array that OpenCV returns when it finds a face: (makes a rectangle) center = [0,0] # Center of the face: a point calculated from the above variable lastface = 0 # int 1-3 used to speed up detection. The script is looking for a right profile face,- # a left profile face, or a frontal face; rather than searching for all three every time,- # it uses this variable to remember which is last saw: and looks for that again. If it- # doesn't find it, it's set back to zero and on the next loop it will search for all three.- # This basically tripples the detect time so long as the face hasn't moved much. scanleft = True # Should we scan for left profiles? scanright = True # should we scan for right profiles? # initialize the camera and grab a reference to the raw camera capture camera = PiCamera() #camera.resolution = (160, 120) #camera.resolution = (640,480) camera.resolution = (1024,768) cameracenter = (camera.resolution[0]/2, camera.resolution[1]/2) camera.framerate = 32 rawCapture = PiRGBArray(camera, camera.resolution) # Points to the last place we sawa a face target = ( camera.resolution[0]/2, camera.resolution[1]/2 ) # Fisheye corrections. See https://medium.com/@kennethjiang/calibrate-fisheye-lens-using-opencv-333b05afa0b0 # 640x480: #correct_fisheye = False #DIM=(640, 480) #K=np.array([[363.787052141742, 0.0, 332.09761373599576], [0.0, 362.23769923959975, 238.35982850966641], [0.0, 0.0, 1.0]]) #D=np.array([[-0.019982864934848042], [-0.10107557279423625], [0.20401597940960342], [-0.1406464201639892]]) # 1024x768: correct_fisheye = True DIM=(1024, 768) K=np.array([[583.6639649321671, 0.0, 518.0139106134624], [0.0, 580.8039721094127, 384.32095600935503], [0.0, 0.0, 1.0]]) D=np.array([[0.0028045742945672475], [-0.14423839478882694], [0.23715105072799644], [-0.1400677375634837]]) #def distance(p0, p1): # return math.sqrt((p0[0] - p1[0])**2 + (p0[1] - p1[1])**2) def search_rightprofile(i): # return profilefaceCascade.detectMultiScale(i,1.3,4,(cv2.CV_HAAR_DO_CANNY_PRUNING + cv2.CV_HAAR_FIND_BIGGEST_OBJECT + cv2.CV_HAAR_DO_ROUGH_SEARCH),(30,30)) if scanright: return profilefaceCascade.detectMultiScale(i, maxSize=(800,800)) else: return () def search_leftprofile(i): if scanleft: revimage = cv2.flip(i, 1) # Flip the image # return profilefaceCascade.detectMultiScale(i,1.3,4,(cv2.CV_HAAR_DO_CANNY_PRUNING + cv2.CV_HAAR_FIND_BIGGEST_OBJECT + cv2.CV_HAAR_DO_ROUGH_SEARCH),(30,30)) return profilefaceCascade.detectMultiScale(i, maxSize=(800,800)) else: return () def search_frontface(i): # return frontalfaceCascade.detectMultiScale(i,1.3,4,(cv2.CV_HAAR_DO_CANNY_PRUNING + cv2.CV_HAAR_FIND_BIGGEST_OBJECT + cv2.CV_HAAR_DO_ROUGH_SEARCH),(30,30)) return frontalfaceCascade.detectMultiScale(i, maxSize=(800,800)) def undistort(i, balance=0.0, dim2=None, dim3=None): # Sanity Check the source dimensions dim1 = i.shape[:2][::-1] #dim1 is the dimension of input image to un-distort assert dim1[0]/dim1[1] == DIM[0]/DIM[1], "Image to undistort needs to have same aspect ratio as the ones used in calibration" if not dim2: dim2 = dim1 if not dim3: dim3 = dim1 scaled_K = K * dim1[0] / DIM[0] # The values of K is to scale with image dimension. scaled_K[2][2] = 1.0 # Except that K[2][2] is always 1.0 # This is how scaled_K, dim2 and balance are used to determine the final K used to un-distort image. OpenCV document failed to make this clear! new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(scaled_K, D, dim2, np.eye(3), balance=balance) map1, map2 = cv2.fisheye.initUndistortRectifyMap(scaled_K, D, np.eye(3), new_K, dim3, cv2.CV_16SC2) return cv2.remap(i, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) def findface(image): global lastface global correct_fisheye faces = (); # TODO: There is a better way to do this. Find it. # First Scan if lastface == 1: faces = search_rightprofile(image) if faces != (): lastface = 1 return faces elif lastface == 2: faces = search_leftprofile(image) if faces != (): lastface = 2 return faces else: faces = search_frontface(image) if faces != (): faceFound=True return faces # Second scan if lastface == 1: faces = search_frontface(image) if faces != (): lastface = 3 return faces elif lastface == 2: faces = search_rightprofile(image) if faces != (): lastface = 1 return faces else: faces = search_leftprofile(image) if faces != (): lastface = 2 return faces # Third scan if lastface == 1: faces = search_leftprofile(image) if faces != (): lastface = 2 return faces elif lastface == 2: faces = search_frontface(image) if faces != (): lastface = 3 return faces else: faces = search_rightprofile(image) if faces != (): lastface = 1 return faces return () def circlefaces(image, faces): global lastface for (x, y, w, h) in faces: cv2.circle(image, (int(x+w/2), int(y+h/2)), int((w+h)/3), (0, 255, 0), 1) # Temporary, save the image cv2.imwrite("tmp/img.{}.facetype{}.png".format(datetime.now().strftime("%Y%m%d.%H%M%S.%f"), lastface), image) def distance_to_closest(faces): # Negative values will be left closestdistance = None for f in faces: x,y,w,h = f centerpoint = (w/2)+x distance = centerpoint - cameracenter[0] if(closestdistance == None or abs(distance) < closestdistance): print("Face closer to center detected. New target location: {} (ctr: {}) - distance: {}".format(centerpoint, cameracenter[0], distance)) closestdistance = distance return closestdistance def stop(): print("Would stop.") return def left(): print('Would go left.') return def right(): print('Would go left.') return def fire(): print("Would fire.") if __name__ == "__main__": # allow the camera to warmup time.sleep(0.1) lastTime = time.time()*1000.0 lastAction = None # capture frames from the camera for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): # grab the raw NumPy array representing the image, then initialize the timestamp # and occupied/unoccupied text print('Time: {}'.format(time.time()*1000.0 - lastTime)) lastTime = time.time()*1000.0 image = frame.array image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # convert to greyscale if correct_fisheye: image = undistort(image, 0.8) faces = findface(image) if faces == (): print("No face found.") stop() lastAction = "Stop" rawCapture.truncate(0) continue circlefaces(image, faces) distance = distance_to_closest(faces) if abs(distance) < 10 and lastAction != "Fire": if lastAction == "Stop": lastAction = "Fire" fire() else: lastAction = "Stop" stop() elif distance < 0 and lastAction != "Left": lastAction = "Left" left() elif distance > 0 and lastAction != "Right": lastAction = "Right" right() else: print("Face found but no action taken. Distance = {}; Last Action = {}".format(distance, lastAction)) # clear the stream in preparation for the next frame rawCapture.truncate(0) # horizontal movement # if abs(travel[0]) < 0.01 and everFound == True: # # Fire! # everFound = False # No face found since last firing # try: # bus.write_byte(SLAVE_ADDRESS, ord(' ')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('F')) # bus.write_i2c_block_data(SLAVE_ADDRESS, ord(' '), [ord('F')]) # print("Sent '{}' to arduino.".format(ord('F'))) # except: # print("Bus I/O error: {}".format(str(e))) # continue # # if travel[0] < 0 and lastmovement != "right": # # Move right # lastmovement = "right" # print("Moving right.") # try: # bus.write_byte(SLAVE_ADDRESS, ord(' ')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('d')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('d')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('d')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('d')) # print("Sent '{}' to arduino.".format(ord('d'))) # except Exception as e: # print("Bus I/O error: {}".format(str(e))) # continue # # if travel[0] > 0 and lastmovement != "left": # # Move left # lastmovement = "left" # print("Moving left.") # try: # bus.write_byte(SLAVE_ADDRESS, ord(' ')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('a')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('a')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('a')) # time.sleep(1) # bus.write_byte(SLAVE_ADDRESS, ord('a')) # time.sleep(1) # print("Sent '{}' to arduino.".format(ord('a'))) # except: # print("Bus I/O error: {}".format(str(e))) # continue