OCT/facetracker.py

#! /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=(30,30))
    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=(30,30))
    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=(30,30))


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), (255, 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
         print("Face found at {},{} with width {} and height {}.".format(x,y,w,h))
         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 send_char(tc):
    try:
        bus.write_byte(SLAVE_ADDRESS, ord(tc))
    except Exception as e:
        print("Bus Error while sending {}: {}".format(tc, str(e)))

def stop():
    print("STOPPING")
    send_char(' ')
    return

def left(distance):
    send_char('a')
    if abs(distance) > 100:
        print('GO LEFT')
        return
    else:
        print('GO LEFT FOR {}s'.format(1.0*abs(distance)/100.0))
        time.sleep(1.0*abs(distance)/100.0)
        stop()

def right(distance=None):
    send_char('d')
    if distance == None:
        print("GO RIGHT")
        return
    else:
        print('GO RIGHT FOR {}s'.format(1.0*abs(distance)/100.0))
        time.sleep(1.0*abs(distance)/100.0)
        stop()

def fire():
    print("FIRING!")
    send_char('F')
    return


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) < 15:
            if lastAction == "Fire":
                # Do nothing
                time.sleep(1)
            elif lastAction == "Stop":
                lastAction = "Fire"
                fire()
                print("Sleeping for 15 seconds...")
                time.sleep(15)
                print("Resuming...")
            else:
                lastAction = "Stop"
                stop()
        elif distance < 0:
            lastAction = "Left"
            left(distance)
        elif distance > 0:
            lastAction = "Right"
            right(distance)
        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