-
Notifications
You must be signed in to change notification settings - Fork 1
/
video_face_matcher_v1.py
437 lines (350 loc) · 15.3 KB
/
video_face_matcher_v1.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
# Author: Rob Hanson
# Required Hardware: Raspberry Pi, Movidius NCS, Adafruit DC and Stepper Motor HAT, LEDs, webcam, 2E10 buggy
#
# Description: This module is used for face recogniton based navigation of an autonomous vehicle
# A frame is read from the webcam every ~500 ms and processed. If a face in the within
# the webcam's field of view it will be detected, cropped and pre-processed, otherwise
# the next frame is read. Then it is converted to a facial feature vector by FaceNet.
#
# The feature vector is compared to that of a target face. If the total error is below
# FACE_MATCH_THRESHOLD, the faces are deemed to be a match.
#
# it is based on Movidius' video_face_matcher.py script with several substantial changes:
# Face detection to align and crop faces
# Proportional only control system for motor control
# Overlaying of bounding boxes on detected and recognised faces
# LED control for monitoring of the current program status
#
#--------------------------------------------------------------------
#-------------------------Library Imports----------------------------
#--------------------------------------------------------------------
from mvnc import mvncapi as mvnc
import numpy
import cv2
import sys
import os
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
import atexit
import time
from gpiozero import LED
#--------------------------------------------------------------------
#-------------------Hyperparameters & Constants----------------------
#--------------------------------------------------------------------
# Scaling factor for face detection cascade, experiment with this value to see what works for your setup
FD_SCALE_FACTOR = 1.3
# The threshold used for face recognition
# Total squared error between two facial feature vectors must be below this for two faces to be deemed a match
# Lower this if false positives occur, increase if false negatives occur
FACE_MATCH_THRESHOLD = 0.7
#EXAMPLES_BASE_DIR='../../'
IMAGES_DIR = './'
TARGET_IMAGES_DIR = IMAGES_DIR + 'target_images/'
# A single image of the target's face named target.jpg is used as the face to recognise
target_image_filename = TARGET_IMAGES_DIR + 'target.jpg'
GRAPH_FILENAME = "facenet_celeb_ncs.graph"
# OpenCV cascade used for face detection. A Haar cascade may also be used
#face_cascade = cv2.CascadeClassifier("""./haarcascade_frontalface_default.xml""")
face_cascade = cv2.CascadeClassifier("""./lbpcascade_frontalface_improved.xml""")
# Proportional only motor controller parameters
ERROR_TOLERANCE = 0.01 # Any feedback error below this value will cause buggy to continue straight
MOTOR_OFFSET_SPEED = 25 # Minimum speed difference between left and right motor while error is greater than tolerance
MOTOR_MAX_SPEED = 100
# Name of the opencv window
CV_WINDOW_NAME = "FaceNet"
# Camera parameters, change these to your camera's resolution but bear in mind that
# higher resolution will decrease frame processing time.
CAMERA_INDEX = 0
#REQUEST_CAMERA_WIDTH = 1280
#REQUEST_CAMERA_HEIGHT = 720
REQUEST_CAMERA_WIDTH = 640
REQUEST_CAMERA_HEIGHT = 480
#--------------------------------------------------------------------
#----------------------Function Definitions--------------------------
#--------------------------------------------------------------------
# Run an inference on the passed image with the FaceNet graph
# Returns 128d facial feature vector
def run_inference(image_to_classify, facenet_graph):
# Scale, convert, and whiten image
resized_image = preprocess_image(image_to_classify)
# Send to NCS
facenet_graph.LoadTensor(resized_image.astype(numpy.float16), None)
# Get output from NCS
output, userobj = facenet_graph.GetResult()
return output
# Whiten an image
def whiten_image(source_image):
source_mean = numpy.mean(source_image)
source_standard_deviation = numpy.std(source_image)
std_adjusted = numpy.maximum(source_standard_deviation, 1.0 / numpy.sqrt(source_image.size))
whitened_image = numpy.multiply(numpy.subtract(source_image, source_mean), 1 / std_adjusted)
return whitened_image
# Rescale to network input shape, convert from BGR to RGB and whiten an image
def preprocess_image(src):
# Scale the image
NETWORK_WIDTH = 160
NETWORK_HEIGHT = 160
preprocessed_image = cv2.resize(src, (NETWORK_WIDTH, NETWORK_HEIGHT))
# Convert to RGB
preprocessed_image = cv2.cvtColor(preprocessed_image, cv2.COLOR_BGR2RGB)
# Whiten
preprocessed_image = whiten_image(preprocessed_image)
return preprocessed_image
# See if two inference outputs correspond to a matching face (calculates total squared error)
def face_match(face1_output, face2_output):
if (len(face1_output) != len(face2_output)):
print('length mismatch in face_match')
return False
total_diff = 0
for output_index in range(0, len(face1_output)):
this_diff = numpy.square(face1_output[output_index] - face2_output[output_index])
total_diff += this_diff
print('Total Difference is: ' + str(total_diff))
if (total_diff < FACE_MATCH_THRESHOLD):
return True
return False
# Handles cv2.waitkey()
# returns False if program should end, or True if should continue
def handle_keys(raw_key):
ascii_code = raw_key & 0xFF
if ((ascii_code == ord('q')) or (ascii_code == ord('Q'))):
return False
return True
# Initialise and run camera
# Run Haar face detection on each frame
# For each face detected, run an inference to see if it's the right face
# If face is recognised, send control signal to motor
def run_camera(target_output, target_image_filename, graph):
# Open camera and set desired resolution
camera_device = cv2.VideoCapture(CAMERA_INDEX)
camera_device.set(cv2.CAP_PROP_FRAME_WIDTH, REQUEST_CAMERA_WIDTH)
camera_device.set(cv2.CAP_PROP_FRAME_HEIGHT, REQUEST_CAMERA_HEIGHT)
# Return camera resolution
actual_camera_width = camera_device.get(cv2.CAP_PROP_FRAME_WIDTH)
actual_camera_height = camera_device.get(cv2.CAP_PROP_FRAME_HEIGHT)
print ('actual camera resolution: ' + str(actual_camera_width) + ' x ' + str(actual_camera_height))
# If camera can't be opened, return an error message
if ((camera_device == None) or (not camera_device.isOpened())):
print ('Could not open camera. Make sure it is plugged in.')
print ('Also, if you installed python opencv via pip or pip3 you')
print ('need to uninstall it and install from source with -D WITH_V4L=ON')
print ('Use the provided script: install-opencv-from_source.sh')
return
# Initialise variables
frame_count = 0
cv2.namedWindow(CV_WINDOW_NAME)
found_match = False
counter = 0
while True:
counter += 1
start = time.time()
# Read video frame from camera
ret_val, vid_frame = camera_device.read()
if (not ret_val):
print("No image from camera, exiting")
break
set_LEDs("red")
# Convert image to grayscale for face detection
gray = cv2.cvtColor(vid_frame, cv2.COLOR_BGR2GRAY)
cv2.imshow(CV_WINDOW_NAME, gray)
# Returns x, y, w & h (bottom left corner, width and height) of rectangles bounding each face
face_locations = face_cascade.detectMultiScale(
gray,
scaleFactor=FD_SCALE_FACTOR,
minNeighbors=5,
minSize=(50, 50),
maxSize=(100, 100),
flags=cv2.CASCADE_SCALE_IMAGE
)
# Get image dimensions
image_height = len(vid_frame)
image_width = len(vid_frame[0])
image_centre = int(image_width/2)
frame_count += 1
frame_name = 'Camera frame #' + str(frame_count)
num_faces = 0
# For each face detected
for (x, y, w, h) in face_locations:
# Increase the size of the bounding box
x = int(max(0,x-0.5*w))
w = 2*w
y = int(max(0, y-0.5*h))
h = 2*h
set_LEDs("yellow")
num_faces += 1
# Run an inference for each face detected
face_image = vid_frame[y:y+h, x:x+w]
test_output = run_inference(face_image, graph)
# Overlay image and update motors
if (face_match(target_output, test_output)):
set_LEDs("green")
# Determine position of the centre of the face in the frame (as a magnitude left/right relative to the centre of the image)
face_centre = [int((y+y+h)/2), int((x+x+w)/2)]
direction = (face_centre[1] - image_centre)/image_centre
# Print result
print('PASS! ', frame_name, ", face ", num_faces, ' matches ', target_image_filename)
# Overlay bounding box and add text of magnitude left or right
cv2.rectangle(vid_frame, (x,y), (x+w, y+h), (0, 255, 0), 2)
cv2.putText(vid_frame, str(direction), (x+5,y+h-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0))
print (direction)
update_motor_speed(direction)
else:
# Face was not a match
found_match = False
print('FAIL! ', frame_name, ", face ", num_faces, ' does not match ', target_image_filename)
cv2.rectangle(vid_frame, (x,y), (x+w, y+h), (0, 0, 255), 2)
if num_faces == 0:
print ("No face detected")
# Turn off all motors
rear_right.run(Adafruit_MotorHAT.RELEASE)
rear_left.run(Adafruit_MotorHAT.RELEASE)
front_right.run(Adafruit_MotorHAT.RELEASE)
front_left.run(Adafruit_MotorHAT.RELEASE)
# check if the window is visible, this means the user hasn't closed
# the window via the X button
prop_val = cv2.getWindowProperty(CV_WINDOW_NAME, cv2.WND_PROP_ASPECT_RATIO)
if (prop_val < 0.0):
print('window closed')
break
# display the results and wait for user to hit a key
cv2.imshow(CV_WINDOW_NAME, vid_frame)
raw_key = cv2.waitKey(1)
if (raw_key != -1):
if (handle_keys(raw_key) == False):
print('user pressed Q')
return
end = time.time()
print ("Time taken to process frame: ", str(end-start)[:5], "s\n")
# Takes float input (-1 [full left] to 1 [full right]) and updates the H bridge accordingly
# Also uses area of bounding box / height of bounding box to determine how fast it should
def update_motor_speed(error):
set_speed = int(MOTOR_MAX_SPEED*0.5)
if error > ERROR_TOLERANCE:
# Turn right
left_speed = set_speed
right_speed = int(max(1 - 2 * error, 0) * set_speed)
elif error < ERROR_TOLERANCE:
# Turn left
right_speed = set_speed
left_speed = int(max(1 + 2 * error, 0) * set_speed)
else:
# Go straight
left_speed = set_speed
right_speed = set_speed
# Update motor state
front_right.setSpeed(right_speed)
rear_right.setSpeed(right_speed)
front_left.setSpeed(left_speed)
rear_left.setSpeed(left_speed)
#print ("Right speed: ", right_speed)
#print ("Left speed: ", left_speed)
rear_right.run(Adafruit_MotorHAT.FORWARD);
rear_left.run(Adafruit_MotorHAT.FORWARD);
front_right.run(Adafruit_MotorHAT.FORWARD);
front_left.run(Adafruit_MotorHAT.FORWARD);
# Turns on the LED colour passed to the function, turns off all others
def set_LEDs(color):
# Turn off all lights
green_LED.off()
yellow_LED.off()
red_LED.off()
# Update LED
if color == "green":
green_LED.on()
#print ("Green")
elif color == "yellow":
yellow_LED.on()
#print ("Yellow")
elif color == "red":
red_LED.on()
#print ("Red")
else:
print ("LEDs powered down")
#--------------------------------------------------------------------
#-----------------------------IO Setup-------------------------------
#--------------------------------------------------------------------
# create a default object, no changes to I2C address or frequency
mh = Adafruit_MotorHAT(addr=0x60)
# Setup each motor
rear_right = mh.getMotor(1) #
rear_left = mh.getMotor(2) #
front_right = mh.getMotor(3) #
front_left = mh.getMotor(4) #
# GPIO PINS
# SDA & SCL in use for motors' PWM
# Using pin 6 as GND for all 3 LEDs
green_LED = LED(12) # Physical pin 32
yellow_LED = LED(16) # Physical pin 36
red_LED = LED(21) # Physical pin 40
#--------------------------------------------------------------------
#----------------------------NCS Setup-------------------------------
#--------------------------------------------------------------------
# Check if NCS is connected
devices = mvnc.EnumerateDevices()
if len(devices) == 0:
print('No NCS devices found')
quit()
# Pick the first stick to run the network
device = mvnc.Device(devices[0])
# Open the NCS
device.OpenDevice()
# The graph file that was created with the ncsdk compiler
graph_file_name = GRAPH_FILENAME
# read in the graph file to memory buffer
with open(graph_file_name, mode='rb') as f:
graph_in_memory = f.read()
# create the NCAPI graph instance from the memory buffer containing the graph file.
graph = device.AllocateGraph(graph_in_memory)
del graph_in_memory
del f
#--------------------------------------------------------------------
#-------------Generate feature vector for target face----------------
#--------------------------------------------------------------------
# Load training image
target_image = cv2.imread(target_image_filename)
# Convert image to grayscale for face detection
gray = cv2.cvtColor(target_image, cv2.COLOR_BGR2GRAY)
# Returns a tuple containing numpy.int32s representing rectangles bounding each face
face_locations = face_cascade.detectMultiScale(
gray,
scaleFactor=1.2,
minNeighbors=5,
minSize=(30, 30),
flags=cv2.CASCADE_SCALE_IMAGE
)
# Run inference on target image
if len(face_locations) == 1:
# Unpack face locations
for (x, y, w, h) in face_locations:
# Increase the boundary around each face
x = int(max(0,x-0.5*w))
w = 2*w
y = int(max(0, y-0.5*h))
h = 2*h
# Run an inference for face
face_image = target_image[y:y+h, x:x+w]
target_output = run_inference(face_image, graph)
print (target_output)
# Overlay bounding box
cv2.rectangle(target_image, (x,y), (x+w, y+h), (0, 255, 0), 2)
cv2.putText(target_image, ("Training face"), (x+5,y+h-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0))
else:
print (len(face_locations), "faces detected in training image.")
print ("Try retaking training image")
quit()
print ("Press any key to continue...")
cv2.imshow("Training image", target_image)
cv2.waitKey(0)
cv2.destroyAllWindows()
run_camera(target_output, target_image_filename, graph)
# Clean up the graph and the device
graph.DeallocateGraph()
device.CloseDevice()
# Turn off all motors
rear_right.run(Adafruit_MotorHAT.RELEASE)
rear_left.run(Adafruit_MotorHAT.RELEASE)
front_right.run(Adafruit_MotorHAT.RELEASE)
front_left.run(Adafruit_MotorHAT.RELEASE)
print ("Motors shutdown")
set_LEDs("")
# Close all windows
cv2.destroyAllWindows()