Course notes: Drone Face Tracking

Preamble

Notes from

See also

Courses

Files required

Packages required

  • opencv-python
  • djitellopy

Notes

New project: TelloFaceTracking

New file:

FaceTrackingTello.py

Utilities.py

Three step

  1. get image
  2. find face
  3. track face

Initialise tello

def initiliseTello():
    myDrone = Tello()
    myDrone.connect()
    myDrone.for_back_velocity = 0
    myDrone.left_right_velocity = 0
    myDrone.up_down_velocity = 0
    myDrone.yaw_velocity = 0
    myDrone.speed = 0
    print(myDrone.get_battery())
    myDrone.streamoff()
    myDrone.streamon()
    return myDrone

connect to tello wi-fi

grab image

def telloGetFrame(theDrone, w=360, h=240):
    myFrame = theDrone.get_frame_read()
    myFrame = myFrame.frame
    img = cv2.resize(myFrame, (w, h))
    return img

main module

# TelloFaceTracking.py

from Utilities import *

import cv2

# w, h = 360, 480
w, h = 1280, 720

myDrone = initiliseTello()

while True:
    # Step 1
    img = telloGetFrame(myDrone, w, h)
    cv2.imshow('Image', img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        myDrone.land()
        break

 

delay when viewing. No delay in processing just the viewing

Video two

Find face, using Viola Jones method using a haarcascade file

haarcascade_frontalface_default.xml from the opencv website.

faces = faceCascde.detectMultiscale(imgGrey, 1.2, 4)

Parameters 1.2 and 4 are scale factor and nearest neighbours.

def findFace(img):
    faceCascde = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    imgGrey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascde.detectMultiscale(imgGrey, 1.2, 4)

    for (x,y,w,h) in faces:
        cv2.rectangle(img, (x,y), (x+w, y+h), (255,0,0), 2)
    return img

Call in the main module with

# Step 2
img = findFace(img)

Test run: shows face detected and rectangle drawn around it.

Now find coordinates to the faces to send to the PID controller.

7:00

What if there are multiple faces? Then track only the face closest to the camera… i.e. the biggest. So make a list of all the faces and store their center points and their areas.

def findFace(img):
    faceCascde = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    imgGrey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascde.detectMultiscale(imgGrey, 1.2, 4)

    myFaceListCenter = []
    myFaceListArea = []

    for (x, y, w, h) in faces:
        cv2.rectangle(img, (x, y), (x + w, y + h), (255, 0, 0), 2)
        cx = x + w // 2
        cy = y + h // 2
        area = w * h
        myFaceListCenter.append([cx, cy])
        myFaceListArea.append(area)

    # Make sure there is at least one face
    if len(myFaceListArea) != 0:
        index = myFaceListArea.index(max(myFaceListArea))
        return img, [myFaceListCenter[index], myFaceListArea[index]]
    else:
        return img[[0, 0], 0]

Change in main module

img, info = findFace(img)
print(f'cx, cy: {info[0][0], info[0][1]}')

Now cx and cy can be used to track the face.

Full code, so far;

# TelloFaceTracking.py

from Utilities import *

import cv2

# w, h = 360, 480
w, h = 1280, 720

myDrone = initiliseTello()

while True:
    # Step 1
    img = telloGetFrame(myDrone, w, h)
    # Step 2
    img = findFace(img)
    img, info = findFace(img)
    print(f'cx, cy: {info[0][0], info[0][1]}')
    cv2.imshow('Image', img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        myDrone.land()
        break

video 3

We only need cx to rotate – we don’t need cy?

Now, cx will fluctuate a lot, so we need to use a PID controller to smooth the movements

def trackFace(theDrone, info, w, pid, pError):

The error is the ± deviation from the width divided by 2, because we always aim to have the face in the center of the frame.

error = cx - 320

or better

error = info[0][0] - w//2

We are controller the speed with the PID (kp, kd and ki)

pid = [0.5, 0.5, 0]

We are not using ki

speed = pid[0]*error + pid[1]*(error-pError)

constrain the speed (this could be used with the robot too)

speed = np.clip(speed, -100, 100)

but it must be an integer to send to the drone

speed = int(np.clip(speed, -100, 100))

So

def trackFace(theDrone, info, w, pid, pError):
    # PID
    error = info[0][0] - w//2
    speed = pid[0]*error + pid[1]*(error-pError)
    # Constrain the speed
    speed = np.clip(speed, -100, 100)

    if info[0][0] != 0:
        theDrone.yaw_velocity = speed
    else:
        theDrone.for_back_velocity = 0
        theDrone.left_right_velocity = 0
        theDrone.up_down_velocity = 0
        theDrone.yaw_velocity = 0
        theDrone.speed = 0
        error = 0
    return error

Now call from main module

# Step 3
pError = trackFace(myDrone, info, w, pError)

You need to set at the top of the file

pError = 0

Need to add the movement to trackFace()

if theDrone.send_rc_control:
    theDrone.send_rc_control(theDrone.for_back_velocity,
                             theDrone.left_right_velocity,
                             theDrone.up_down_velocity,
                             theDrone.yaw_velocity)

 

In the main module, we need to make the drone take off, in the loop

# Flight
if start_counter == 0:
    myDrone.takeoff()
    start_counter = 1

At the top

start_counter = 0  # for no flight set to 1, for flight set to 0

That is it.

Full code (main)

# TelloFaceTracking.py

from Utilities import *

import cv2

# w, h = 360, 480
w, h = 1280, 720

pid = [0.5, 0.5, 0]
pError = 0
start_counter = 0  # for no flight set to 1, for flight set to 0

myDrone = initiliseTello()

while True:

    # Flight
    if start_counter == 0:
        myDrone.takeoff()
        start_counter = 1

    # Step 1
    img = telloGetFrame(myDrone, w, h)
    # Step 2
    # img = findFace(img)
    img, info = findFace(img)
    print(f'cx, cy: {info[0][0], info[0][1]}')
    # Step 3
    pError = trackFace(myDrone, info, w, pid, pError)
    cv2.imshow('Image', img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        myDrone.land()
        break

Full code (utilities)

# Utilities.py

from djitellopy import Tello
import cv2
import numpy as np


def initiliseTello():
    myDrone = Tello()
    myDrone.connect()
    myDrone.for_back_velocity = 0
    myDrone.left_right_velocity = 0
    myDrone.up_down_velocity = 0
    myDrone.yaw_velocity = 0
    myDrone.speed = 0
    print(myDrone.get_battery())
    myDrone.streamoff()
    myDrone.streamon()
    return myDrone


def telloGetFrame(theDrone, w=360, h=240):
    myFrame = theDrone.get_frame_read()
    myFrame = myFrame.frame
    img = cv2.resize(myFrame, (w, h))
    return img


def findFace(img):
    faceCascde = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    imgGrey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascde.detectMultiscale(imgGrey, 1.2, 4)

    myFaceListCenter = []
    myFaceListArea = []

    for (x, y, w, h) in faces:
        cv2.rectangle(img, (x, y), (x + w, y + h), (255, 0, 0), 2)
        cx = x + w // 2
        cy = y + h // 2
        area = w * h
        myFaceListCenter.append([cx, cy])
        myFaceListArea.append(area)

    # Make sure there is at least one face
    if len(myFaceListArea) != 0:
        index = myFaceListArea.index(max(myFaceListArea))
        return img, [myFaceListCenter[index], myFaceListArea[index]]
    else:
        return img, [[0, 0], 0]

def trackFace(theDrone, info, w, pid, pError):
    # PID - yaw
    error = info[0][0] - w // 2
    speed = pid[0] * error + pid[1] * (error - pError)
    # Constrain the speed
    speed = int(np.clip(speed, -100, 100))
    print(speed)

    if info[0][0] != 0:
        theDrone.yaw_velocity = speed
    else:
        theDrone.for_back_velocity = 0
        theDrone.left_right_velocity = 0
        theDrone.up_down_velocity = 0
        theDrone.yaw_velocity = 0
        theDrone.speed = 0
        error = 0

    if theDrone.send_rc_control:
        theDrone.send_rc_control(theDrone.for_back_velocity,
                                 theDrone.left_right_velocity,
                                 theDrone.up_down_velocity,
                                 theDrone.yaw_velocity)
    return error

Moving forwards and back

For forwards and backwards do a PID on the area. Let’s say the area should be 10,000 pixels. If less then move forward, and if more then move backwards. Repeat these lines, but for area and not the width:

# PID
error = info[0][0] - w // 2
speed = pid[0] * error + pid[1] * (error - pError)
# Constrain the speed
speed = int(np.clip(speed, -100, 100))
print(speed)

So full code would be (note the commented out lines for pError_area)

def trackFace(theDrone, info, w, pid, pError, pError_area):
    # PID - yaw
    error = info[0][0] - w // 2
    speed = pid[0] * error + pid[1] * (error - pError)
    # Constrain the speed
    speed = int(np.clip(speed, -100, 100))
    print(speed)
    # PID - forward back
    area_ideal = 1000
    error_area = info[1] - area_ideal
    speed_forward = pid[0] * error_area + pid[1] * (error_area - pError_area)
    # Constrain the speed
    speed_forward = int(np.clip(speed_forward, -100, 100))
    print(speed_forward)

    if info[0][0] != 0:
        theDrone.yaw_velocity = speed
        theDrone.for_back_velocity = speed_forward

    else:
        theDrone.for_back_velocity = 0
        theDrone.left_right_velocity = 0
        theDrone.up_down_velocity = 0
        theDrone.yaw_velocity = 0
        theDrone.speed = 0
        error = 0

    if theDrone.send_rc_control:
        theDrone.send_rc_control(theDrone.for_back_velocity,
                                 theDrone.left_right_velocity,
                                 theDrone.up_down_velocity,
                                 theDrone.yaw_velocity)
    return error, error_area

In main you would need to change for the return values

pError, pError_area = trackFace(myDrone, info, w, pid, pid_area, pError, pError_area)

and initialise the previous area error to zero

pError_area = 0

Note that you might need different pid values for the area, so pid_area

 

This is the end, my friend

 

 

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s