Introduction:

In this project we will see how we can use the power of image processing and simple mechanics (Pan and Tilt Mechanism ) to track any Face so that the face is always at the centre of the camera feed.

We can certainly track any desired object instead of a face (shown in this post). Also we can program to track only a particular persons face using Face recognition Feature.

Let’s have a look at the Demo working of the project:

CODE:

The entire code for this project is available at this GitHub Repository: https://github.com/htgdokania/FaceTrack_PiCam

Parts required:

• Raspberry pi
• Pi Camera
• Two micro Servos
• Any Pan and Tilt Mechanism.(Refer Here)

Structure/Work Flow:

• Step1: Setup up Pi camera along with Pan and Tilt Mechanism.
• Step2: Do the Servo connections along with Pi camera cable attachment.
• Step3: Write a code to control the servo movement servomove.py
• Step4: Write the main.py code
  • Start Reading Frames from Pi Camera.
  • Detect Face in the current frame and get its coordinates
  • Calculate the deviation of the face from the centre and set it as error
  • implement a Proportional controller to calculate value to change the servo position
  • send the values to the servo movement function to get the face back at centre.

Step1: Setup up Pi camera along with Pan and Tilt.

• First mount the camera on the pan and tilt mechanism along with the servos.It should look like this.

• Make Sure the camera is moving freely on both the x and y axis.Something like this:

Step2: Do the Servo connections along with Pi camera cable.

• connect the Two servo signal pins to any of the two GPIO pins of your choice.
• Power the Servos using 5v supply of pi . Alternatively we can power it using a usb power supply.

Step3: Write a code to control the servo movement (servomove.py)

• Lets Define servomove.py
• First import libraries

import RPi.GPIO as GPIO          
from time import sleep

• Next define the signal pins of the the servo motors and initialize them at 50hz (as per servo documentation):

ypin = 11
xpin = 13

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(xpin,GPIO.OUT)
GPIO.setup(ypin,GPIO.OUT)

x=GPIO.PWM(xpin,50)
y=GPIO.PWM(ypin,50)

Note: Generally Servo motors can move  from 0 to 180 degrees.
 > 0 at 1ms PWM signal at Duty Cycle 5 for 50 Hz.
 > 180 at 2ms PWM signal at Duty Cycle 10 for 50 Hz.

Next we define a class within which we define few functions to control x and y position.

Initially we set the servo duty cycle so that it is at the centre at 90 degree .Adjust the duty cycle to bring at centre.

class servopos():
    def __init__(self):
        self.currentx,self.currenty=7,4
        x.start(self.currentx)
        y.start(self.currenty)
        sleep(1)
        x.ChangeDutyCycle(0)
        y.ChangeDutyCycle(0)
            
    def setposx(self,diffx):
        self.currentx+=diffx
        self.currentx=round(self.currentx,2)
        if(self.currentx<15 and self.currentx>0):
            x.ChangeDutyCycle(self.currentx)  
    def setposy(self,diffy):
        self.currenty+=diffy
        self.currenty=round(self.currenty,2)
        if(self.currenty<15 and self.currenty>0):
            y.ChangeDutyCycle(self.currenty)  
    def setdcx(self,dcx):
        x.ChangeDutyCycle(dcx)  
    def setdcy(self,dcy):
        y.ChangeDutyCycle(dcy)

Step4: Write the main.py code

• First import the required libraries.

# import the necessary packages
from picamera.array import PiRGBArray
from picamera import PiCamera
import time 
import cv2
from servomove import servopos
ser=servopos() # declare object of class servopos()

• load the Haar cascade file to detect face

face_cascade= cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

• Set the PID values for x and y axis movement. we set I,D values as 0 for proportional controller.

Px,Ix,Dx=-1/160,0,0
Py,Iy,Dy=-0.2/120,0,0

#initialize some other required vaqriables
integral_x,integral_y=0,0
differential_x,differential_y=0,0
prev_x,prev_y=0,0

• Set the Pi camera parameters to load frames in an optimised way:

width,height=320,240
camera = PiCamera()
camera.resolution = (width,height)
camera.framerate = 30
rawCapture = PiRGBArray(camera, size=(width,height))
time.sleep(1)

Start Reading Frames from Pi Camera.

• Inside a for loop read frames continuously using camera.capture_continuous()
• Store image array as image variable and flip it to correct camera orientation.
• Also set both the servos duty cycle to 0 (This will ensure servos are stable and not vibrate).

for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
    image = frame.array
    frame=cv2.flip(image,1)
    ser.setdcx(0)
    ser.setdcy(0)

Detect Face in the current frame and get its coordinates

• Detect Faces using the haar Cascade file we loaded earlier.
• Get the face coordinates of the first face detected.

    gray=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)    
    #detect face coordinates x,y,w,h
    faces=face_cascade.detectMultiScale(gray,1.3,5) # face_cascade loaded earlier.
    c=0
    for(x,y,w,h) in faces:
        c+=1
        if(c>1):  # we just take care of the first face detected.
            break

Calculate the deviation of the face from the centre and set it as error

• The deviation of the face centre coordinates from the centre of the frame it the error.
• Next,calculate integral,differential values and set the new face coordinates as previous coordinates for next iteration.

        #centre of face
        face_centre_x=x+w/2
        face_centre_y=y+h/2
        
        #calculate pixels to move 
        error_x=160-face_centre_x  # X-coordinate of Centre of frame is 160
        error_y=120-face_centre_y # Y-coordinate of Centre of frame is 120
        
        integral_x=integral_x+error_x
        integral_y=integral_y+error_y
        
        differential_x= prev_x- error_x
        differential_y= prev_y- error_y
        
        prev_x=error_x
        prev_y=error_y

Implement a Proportional controller to calculate value to change the servo position

• Finally Calculate the value for x movement and y movement using PID logic.(valx,valy)

        valx=Px*error_x +Dx*differential_x + Ix*integral_x
        valy=Py*error_y +Dy*differential_y + Iy*integral_y
        
        valx=round(valx,2) #round off to 2 decimel points.
        valy=round(valy,2)

Send the values to the servo movement function to get the face back at centre.

        if abs(error_x)<20:
            ser.setdcx(0)
        else:
            if abs(valx)>0.5:
                sign=valx/abs(valx)
                valx=0.5*sign
            ser.setposx(valx)

        if abs(error_y)<20:
            ser.setdcy(0)
        else:
            if abs(valy)>0.5:
                sign=valy/abs(valy)
                valy=0.5*sign
            ser.setposy(valy)

Display:

• Finally we draw a box around the detected face and display the frame for visualization purpose.

        if(c==1):
            frame=cv2.rectangle(frame,(x,y),(x+w,y+h),(255,0,0),6)

    cv2.imshow('frame',frame) #display image

• Set a logic to exit fromt the loop and close all open windows, if user presses ‘q’ on keyboard.

    key = cv2.waitKey(1) & 0xFF
    rawCapture.truncate(0)
    if key == ord("q"):
        break

cv2.destroyAllWindows()

That’s All .Now run the main.py code to see it in action.All you need to do is moe your face and see how your bot keeps tracking it continuosly.