def tra_gh_filter(trajectory, v, avg_dis=152.5781, var_dis=327.7119):
point_list = []
for point in list(trajectory.coords):
point_list.append(Point(point))
point_num = len(point_list)
distance_list = []
for i in range(point_num - 1):
dis = point_list[i].distance(point_list[i + 1])
distance_list.append(dis)
error_list = []
for i in range(len(distance_list)):
if distance_list[i] > avg_dis + 3 * var_dis:
error_list.append(i)
error_point = []
if len(error_list) > 1:
for i in range(len(error_list) - 1):
error_dis_now = error_list[i]
error_dis_next = error_list[i + 1]
if error_dis_next == error_dis_now + 1:
error_index = error_dis_next
error_point.append(error_dis_next)
elif len(error_list) == 1:
if error_list[0] == point_num - 1:
error_index = point_num - 1
error_point.append(error_index)
x_0 = np.array([point_list[0].x, point_list[0].y])
dx_0 = np.array(v)
gh_f_tra = GHFilter(x=x_0, dx=dx_0, dt=10., g=.8, h=.2)
correct_point = [point_list[0]]
for i in range(1, len(point_list)):
if i in error_point:
gh_f_tra.g = 0
gh_f_tra.h = 0
else:
gh_f_tra.g = .8
gh_f_tra.h = .2
gh_f_tra.update(z=np.array([point_list[i].x, point_list[i].y]))
correct_point.append(gh_f_tra.x)
line_correct = LineString(correct_point)
return line_correct
def test_GHFilterOrder():
def fx(x):
return 2 * x + 1
f1 = GHFilterOrder(x0=array([0, 0]), dt=1, order=1, g=.6, h=.02)
f2 = GHFilter(x=0, dx=0, dt=1, g=.6, h=.02)
for i in range(100):
z = fx(i) + randn()
f1.update(z)
f2.update(z)
assert abs(f1.x[0] - f2.x) < 1.e-18
def foo():
def fx(x):
return 2*x+1
f1 = GHFilterOrder(x0=array([0,0]), dt=1, order=1, g=.6, h=.02)
f2 = GHFilter(x=0, dx=0, dt=1, g=.6, h=.02)
for i in range(100):
z = fx(i) + randn()
f1.update(z)
f2.update(z)
assert abs(f1.x[0]-f2.x) < 1.e-18
def test_1d_array():
f1 = GHFilter (0, 0, 1, .8, .2)
f2 = GHFilter (array([0]), array([0]), 1, .8, .2)
str(f1)
str(f2)
# test both give same answers, and that we can
# use a scalar for the measurment
for i in range(1,10):
f1.update(i)
f2.update(i)
assert f1.x == f2.x[0]
assert f1.dx == f2.dx[0]
assert f1.VRF() == f2.VRF()
# test using an array for the measurement
s1 = Saver(f1)
s2 = Saver(f2)
for i in range(1,10):
f1.update(i)
f2.update(array([i]))
s1.save()
s2.save()
assert f1.x == f2.x[0]
assert f1.dx == f2.dx[0]
assert f1.VRF() == f2.VRF()
s1.to_array()
s2.to_array()
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
gh = GHFilter(x=0, dx=0, dt=1, g=0.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x + random.randn() for x in range(0, 100)]
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2 * (2 * i + 1) / ((i + 2) * (i + 1))
h = 6 / ((i + 2) * (i + 1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.0e-13)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x - y
assert r < 1.0e-8
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x + random.randn() for x in range(0, 100)]
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2 * (2 * i + 1) / ((i + 2) * (i + 1))
h = 6 / ((i + 2) * (i + 1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.e-13)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x - y
assert r < 1.e-8
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
global lsq, lsq2, xs, lsq_xs
gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x+random.randn()*10 for x in range(0, 10000)]
# test __repr__ at least doesn't crash
try:
str(lsq2)
except:
assert False, "LeastSquaresFilter.__repr__ exception"
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2*(2*i + 1) / ((i+2)*(i+1))
h = 6 / ((i+2)*(i+1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.e-10), '{}, {}, {}'.format(
i, x2[0], lx)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x-y
assert r < 1.e-8
def test_2d_array():
""" test using 2 independent variables for the
state variable.
"""
f = GHFilter(array([0, 1]), array([0, 0]), 1, .8, .2)
f0 = GHFilter(0, 0, 1, .8, .2)
f1 = GHFilter(1, 0, 1, .8, .2)
# test using scalar in update (not normal, but possible)
for i in range(1, 10):
f.update(i)
f0.update(i)
f1.update(i)
assert f.x[0] == f0.x
assert f.x[1] == f1.x
assert f.dx[0] == f0.dx
assert f.dx[1] == f1.dx
# test using array for update (typical scenario)
f = GHFilter(array([0, 1]), array([0, 0]), 1, .8, .2)
f0 = GHFilter(0, 0, 1, .8, .2)
f1 = GHFilter(1, 0, 1, .8, .2)
for i in range(1, 10):
f.update(array([i, i + 3]))
f0.update(i)
f1.update(i + 3)
assert f.x[0] == f0.x
assert f.x[1] == f1.x
assert f.dx[0] == f0.dx
assert f.dx[1] == f1.dx
assert f.VRF() == f0.VRF()
assert f.VRF() == f1.VRF()
def test_2d_array():
""" test using 2 independent variables for the
state variable.
"""
f = GHFilter(array([0,1]), array([0,0]), 1, .8, .2)
f0 = GHFilter(0, 0, 1, .8, .2)
f1 = GHFilter(1, 0, 1, .8, .2)
# test using scalar in update (not normal, but possible)
for i in range (1,10):
f.update (i)
f0.update(i)
f1.update(i)
assert f.x[0] == f0.x
assert f.x[1] == f1.x
assert f.dx[0] == f0.dx
assert f.dx[1] == f1.dx
# test using array for update (typical scenario)
f = GHFilter(array([0,1]), array([0,0]), 1, .8, .2)
f0 = GHFilter(0, 0, 1, .8, .2)
f1 = GHFilter(1, 0, 1, .8, .2)
for i in range (1,10):
f.update (array([i, i+3]))
f0.update(i)
f1.update(i+3)
assert f.x[0] == f0.x
assert f.x[1] == f1.x
assert f.dx[0] == f0.dx
assert f.dx[1] == f1.dx
assert f.VRF() == f0.VRF()
assert f.VRF() == f1.VRF()
####################################################
init_pos_guess = np.array([x_0,y_0])
init_vel_guess = np.array([1,1])
flag = True
while(flag):
# g,h = np.random.uniform(0,0.2),np.random.uniform(0,0.01)
g, h = 0.020282406381970187, 0.0003965804370818338
# Just noticed those were good, I'll keep the random uniform distributions up for tests later
print('g =',g, '\th =',h)
GH_air = GHFilter(x= init_pos_guess, dx = init_vel_guess, dt = 0.001, g = g, h= h) # g = .1, h = .005
Data = []
for i in range(count):
# print(i)
GH_air.update(z = np.array([x_data[i], y_data[i]])) # x, y
Data.append(GH_air.x)
# LATER, try and use GH_air.batch_filter([[],[],[],...,[]]) to update a whole batch of data instead of for loops
Data = np.array(Data)
print(count)
# print(Data)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# ax.plot3D(x_data,time,y_data, 'red')
ax.scatter3D(x_data,time,y_data, alpha = 0.1, c = 'b') # alpha changes transparency
#!/usr/bin/env python3
'''
H:向前移动
J:向上移动
K:向下移动
L:向后移动
'''
from filterpy.gh import GHFilter
f = GHFilter(x=0., dx=0., dt=1., g=.8, h=.2)
f.update(z=1.2)
print("f.update = ", f.update(z=1.2))
print("f.x = ", f.x, ";", "f.dx = ", f.dx)
print(f.update(z=2.1, g=.85, h=.15))
class Tuner:
def __init__(self, sampleRate=44100,startFreq=10,stopFreq=1200,fftLen=4096,
dbgTimeStart=0.,dbgTimeLen=0.,blockSize=1024,note='E2'):
self.sampleRate = sampleRate
self.fftLen = fftLen
self.peak = 0.0
self.avgCoeff = 1.
self.blockSize = blockSize
self.dbgTimeStart = dbgTimeStart
self.dbgTimeLen = dbgTimeLen
self.agc = AGC()
self.note = note
self.power = 0;
self.tracker = GHFilter(x=noteFreqs[note], dx=0, dt=1., g=.05, h=.005)
self.trackLoc = []
def readStream(self):
sampleType = pyaudio.paInt16
channels = 2
self.sampleRate = 8000
self.filt = BandPassFilter(sampleRate=self.sampleRate,stopFreq=noteFreqs[self.note]);
# start streaming samples from the soundcard
audio = pyaudio.PyAudio()
stream = audio.open(format=pyaudio.paInt16, channels=1,
rate=self.sampleRate, input=True,
frames_per_buffer=self.blockSize)
while True:
data = stream.read(self.blockSize)
data_d = np.fromstring(data, dtype=np.int16)
data_d = np.reshape(data_d, (self.blockSize, 1))
peak, power = self.processSamples_autocorr(data_d[:,0]/32768.)
print(peak,power)
def readWave(self, filename):
w = wave.open(filename)
self.sampleRate = w.getframerate()
numSamples = w.getnframes()
numChannels = w.getnchannels()
sampleWidth = w.getsampwidth()
d = w.readframes(numSamples)
# TODO - handle bitwidth, assumes 16bits for now
d = np.fromstring(d, dtype=np.int16)
d = np.reshape(d, (numSamples, numChannels))
d = d/32768
sampleCount = 0
sample = []
ploc = []
pwr =[]
self.filt = BandPassFilter(sampleRate=self.sampleRate,stopFreq=noteFreqs[self.note]*2.);
while sampleCount + self.blockSize < numSamples:
x = d[sampleCount:sampleCount+self.blockSize-1,0]
#p, power = self.processSamples_hps(x)
p, power = self.processSamples_autocorr(x)
ploc.append(p)
pwr.append(power)
sample.append(sampleCount/self.sampleRate)
sampleCount += self.blockSize
time = sampleCount/self.sampleRate
if time > self.dbgTimeStart and time < self.dbgTimeStart + self.dbgTimeLen:
print(time,p)
plt.figure()
plt.plot(x)
#plt.figure()
#plt.plot(self.f,10*np.log10(self.Pxx))
#plt.figure()
#plt.plot(self.f,10*np.log10(self.PxxH))
#plt.figure()
#pxx = np.fft.ifft(self.Pxx)
#t=np.arange(0,(self.fftLen/2)+1)/(self.sampleRate/2)
#plt.plot(t,pxx)
plt.figure()
plt.plot(self.xx)
plt.plot(self.fit_range, self.xx_fit)
plt.show()
fig, ax1 = plt.subplots()
ax1.plot(sample,ploc,'r*')
ax1.plot(sample,self.trackLoc,'r')
ax2 = ax1.twinx()
ax2.plot(sample,pwr,'b')
temp = np.array(pwr)
temp = np.append(temp,0)
ax2.plot(sample,np.abs(np.diff(temp)),'g')
plt.figure()
cents = 1200*np.log2(np.array(self.trackLoc)/noteFreqs[self.note])
plt.plot(sample,cents)
plt.show()
print('HI')
def processSamples_hps(self, x):
# bandpass filter the signal
x = self.filt.filter(x)
x = self.agc.run(x)
power = np.sum(x**2)/len(x)
power = 10.*np.log10(power)
# estimate the power spectrum of the signal
self.f,self.Pxx = signal.periodogram(x,self.sampleRate,window='hanning',nfft=self.fftLen,scaling='spectrum')
# create the harmonic spectrum
self.PxxH = self.harmonicProductSpectrum(self.Pxx)
fundamental = np.argmax(self.PxxH[0:np.uint16(len(self.PxxH)/3)])
# estimate the peak frequency
peakLoc, peakPower = self.interpPeak(self.Pxx,fundamental)
if power < -3. or power > 3.0:
peakLoc = 0
else:
peakLoc *= self.sampleRate/self.fftLen
return peakLoc, power
def peak_fit(self,x,a,b,c):
return a*x**2 + b*x +c
def processSamples_autocorr(self, x):
power = np.sum(x**2)/len(x)
power = 10.*np.log10(power)
# run the automatic gain control algorithm to normalize the signal level
# this should improve detecting the fundamental frequency in the
# autocorrelation sequence
#x = self.agc.run(x)
x = self.filt.filter(x)
X = np.fft.fft(x,n=self.fftLen)
freq = np.fft.fftfreq(len(X), 1/self.sampleRate)
i = freq > 0
#plt.figure()
#plt.plot(freq[i],np.abs(X[i]))
XX = X*X.conj()
xx = np.fft.ifft(XX,n=self.fftLen).real
self.xx = xx
# determine the range to search in the autocorr sequence
stringFreqLo = noteFreqs[self.note]*2**(-2/12)
stringFreqHi = noteFreqs[self.note]*2**(2/12)
hi = np.int(self.sampleRate/stringFreqLo)
lo = np.int(self.sampleRate/stringFreqHi)
tt = np.argmax(xx[lo:hi])
tt += lo
#fit a parabola to interpolate the peak
xdata = np.arange(-10,10)
popt,_ = curve_fit(self.peak_fit, xdata, xx[tt+xdata])
self.fit_range = np.arange(-10,9,.01)
self.xx_fit = self.peak_fit(self.fit_range,popt[0],popt[1],popt[2])
fit_peak_loc = np.argmax(self.xx_fit)
peakLoc = self.sampleRate/(self.fit_range[fit_peak_loc]+tt)
if fit_peak_loc+1 >= len(self.xx_fit):
peakLoc = 0
elif self.xx_fit[fit_peak_loc] < self.xx_fit[fit_peak_loc+1]:
peakLoc = 0
if power < -50.:
peakLoc = 0
#only use parts of the waveform where its not rising too quickly
if np.abs(power - self.power) > 5:
peakLoc = 0
self.power = power
if peakLoc == 0:
self.tracker.update(self.tracker.x)
self.tracker.dx = 0
else:
self.tracker.update(z=peakLoc)
self.trackLoc.append(self.tracker.x)
return peakLoc, power
def interpPeak(self, Pxx, index):
y1 = Pxx[index-1]
y2 = Pxx[index]
y3 = Pxx[index+1]
offset = (y3-y1)/(y1 + y2 + y3)
peakLoc = index + offset
power = (y1 + y2 + y3)/3.0
return peakLoc,power
def harmonicProductSpectrum(self, Pxx):
temp = np.copy(Pxx)
for level in (2,3,4,5):
N = np.int(len(Pxx)/level)
for index in np.arange(0,N):
temp[index] *= Pxx[index*level]
return temp
def testSignal(self, testFreq):
x1=np.cos(2.0*np.pi*(testFreq/self.sampleRate)*np.arange(0,self.sampleRate-1));
x2=1.0*np.cos(2.0*np.pi*(testFreq*2./self.sampleRate)*np.arange(0,self.sampleRate-1));
x3=1.0*np.cos(2.0*np.pi*(testFreq*3./self.sampleRate)*np.arange(0,self.sampleRate-1));
x = x1+x2+x3
return x
class Face(object):
EYE_AR_THRESH = 0.20
EYE_AR_CONSEC_FRAMES = 3
name = 'unknown'
pitch_limit = [-20, 20]
yaw_limit = [-30, 30]
init_life_count = 5
smileCascade = cv2.CascadeClassifier(smilePath)
def __init__(self, face_rect, img):
self.face_rect = face_rect
self.last_time_eyes_open = time.time()
self.last_time_see_road = time.time()
self.names = {}
self.play_alarm = False
self.full_img = img
self.gaze_filter = GHFilter(x=0, dx=np.array([0]), dt=1, g=.6, h=.02)
self.skip_frame = 0
self.is_driver = False
self.start_yawn = False
self.life_counter = self.init_life_count
self.eye_counter = 0
self.yawn_count = 0
self.alive = True
self.born_time = time.time()
if audio:
self.music = vlc.MediaPlayer('rooster.mp3')
def life_reset(self):
self.life_counter = self.init_life_count
def match(self, face_rects, img):
self.full_img = img
min_dist = 1000
threshold = 100
cur_center = self.face_rect.center()
ind = -1
for i in range(len(face_rects)):
face_rect = face_rects[i]
face_center = face_rect.center()
#find which one is closest to current face_rect we know
distance = abs(cur_center.x - face_center.x) + abs(cur_center.y -
face_center.y)
if distance < min(threshold, min_dist):
min_dist = distance
ind = i
if ind == -1:
self.life_counter -= 1
if self.life_counter == 0:
self.alive = False
else:
self.life_reset()
self.face_rect = face_rects[ind]
face_rects.pop(ind)
return face_rects
def update(self, face_img, shape, euler_angle):
self.img = face_img
row, col = face_img.shape[:2]
#check who is this, do this in skip to save computation
if self.skip_frame % 10 == 0:
name = self.classify_face(face_img)
if name != 'unknown':
self.name = name
self.skip_frame = 0
self.skip_frame += 1
cv2.putText(self.img,
str(self.name), (10, 10),
cv2.FONT_HERSHEY_PLAIN,
1.0, (0, 255, 0),
thickness=1)
euler_angle = np.round(euler_angle, 1)
rpy = str(euler_angle[2, 0]) + ', ' + str(
euler_angle[0, 0]) + ', ' + str(euler_angle[1, 0])
cv2.putText(self.img,
rpy, (10, self.img.shape[0] - 10),
cv2.FONT_HERSHEY_PLAIN,
0.7, (0, 255, 0),
thickness=1)
elapsed_time = str(round(time.time() - self.born_time, 1))
cv2.putText(self.img,
elapsed_time,
(self.img.shape[1] - 40, self.img.shape[0] - 10),
cv2.FONT_HERSHEY_PLAIN,
0.7, (0, 255, 0),
thickness=1)
gaze_direction, leftEye_patch, rightEye_patch = self.check_gaze(shape)
cv2.putText(self.img,
"gaze: " + str(gaze_direction),
(10, self.img.shape[0] - 25),
cv2.FONT_HERSHEY_PLAIN,
0.7, (0, 255, 0),
thickness=1)
self.img[:leftEye_patch.shape[0],
int(col / 2) -
leftEye_patch.shape[1]:int(col / 2)] = cv2.cvtColor(
leftEye_patch, cv2.COLOR_GRAY2BGR)
self.img[:rightEye_patch.shape[0],
int(col / 2):int(col / 2) +
rightEye_patch.shape[1]] = cv2.cvtColor(
rightEye_patch, cv2.COLOR_GRAY2BGR)
#check head pose if seeing road
if self.see_road(euler_angle, gaze_direction):
cv2.putText(self.img,
"seeing road", (10, 25),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 255, 0),
thickness=1)
self.last_time_see_road = time.time()
not_seeing_road_elapsed_time = 0.0
else:
not_seeing_road_elapsed_time = round(
(time.time() - self.last_time_see_road), 1)
cv2.putText(self.img,
"not seeing road: " +
str(not_seeing_road_elapsed_time), (10, 25),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 0, 255),
thickness=1)
#check status of eyes
if self.eyes_open(shape):
cv2.putText(self.img,
"eyes_open", (10, 40),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 255, 0),
thickness=1)
self.last_time_eyes_open = time.time()
closed_eyes_elapsed_time = 0.0
else:
closed_eyes_elapsed_time = round(
(time.time() - self.last_time_eyes_open), 1)
cv2.putText(self.img,
"eyes closed: " + str(closed_eyes_elapsed_time),
(10, 40),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 0, 255),
thickness=1)
if audio:
if not_seeing_road_elapsed_time > 3.0 or closed_eyes_elapsed_time > 3.0:
self.music.play()
else:
self.music.stop()
#check status of eyes
mouth_status = self.mouth_status(shape)
if mouth_status == "yawn":
if not self.start_yawn:
self.yawn_count += 1
self.start_yawn = True
cv2.putText(self.img,
mouth_status + ': ' + str(self.yawn_count), (10, 55),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 0, 255),
thickness=1)
else:
if mouth_status == "mouth closed":
self.start_yawn = False
cv2.putText(self.img,
mouth_status + ', yawn: ' + str(self.yawn_count),
(10, 55),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 255, 0),
thickness=1)
smile = self.detect_smile()
# Set region of interest for smiles
for (x, y, w, h) in smile:
cv2.putText(self.img,
'smile ', (10, 70),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 255, 255),
thickness=1)
# cv2.rectangle(self.img, (x, y), (x+w, y+h), (255, 0, 0), 1)
brow = self.brow_status(shape)
if brow == 5:
#sad
brow_text = 'sad'
elif brow == 3:
#suprise
brow_text = 'brow raised'
elif brow == 7:
#fear
brow_text = 'fear'
elif brow == 4:
#anger
brow_text = 'tense'
else:
#neutral
brow_text = ''
cv2.putText(self.img,
brow_text, (10, 85),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 255, 255),
thickness=1)
if self.in_distress(shape):
cv2.putText(self.img,
'distress ', (10, 100),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 255, 255),
thickness=1)
def detect_smile(self):
roi_gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)
smile = self.smileCascade.detectMultiScale(
roi_gray,
scaleFactor=1.7,
minNeighbors=22,
minSize=(25, 25),
flags=cv2.CASCADE_SCALE_IMAGE)
return smile
def see_road(self, euler_angle, gaze_direction):
#check the head yaw and pitch only
pitch = euler_angle[0, 0]
yaw = euler_angle[1, 0] + gaze_direction
if pitch > self.pitch_limit[0] and pitch < self.pitch_limit[1]:
if yaw > self.yaw_limit[0] and yaw < self.yaw_limit[1]:
return True
return False
def eyes_open(self, shape):
(rStart, rEnd) = face_utils.FACIAL_LANDMARKS_IDXS["left_eye"]
(lStart, lEnd) = face_utils.FACIAL_LANDMARKS_IDXS["right_eye"]
leftEye = shape[lStart:lEnd]
rightEye = shape[rStart:rEnd]
leftEAR = self.eye_aspect_ratio(leftEye)
rightEAR = self.eye_aspect_ratio(rightEye)
ear = (leftEAR + rightEAR) / 2.0
if ear < self.EYE_AR_THRESH:
self.eye_counter = 0
return False
else:
# if the eyes were closed for a sufficient number of
self.eye_counter += 1
if self.eye_counter > self.EYE_AR_CONSEC_FRAMES:
return True
else:
return False
def check_gaze(self, shape):
#check gaze of eyes
(rStart, rEnd) = face_utils.FACIAL_LANDMARKS_IDXS["left_eye"]
(lStart, lEnd) = face_utils.FACIAL_LANDMARKS_IDXS["right_eye"]
leftEye = shape[lStart:lEnd]
rightEye = shape[rStart:rEnd]
xmin = np.clip(np.amin(leftEye[:, 0]), 0, self.full_img.shape[1] - 1)
ymin = np.clip(np.amin(leftEye[:, 1]), 0, self.full_img.shape[0] - 1)
xmax = np.clip(np.amax(leftEye[:, 0]), 0, self.full_img.shape[1] - 1)
ymax = np.clip(np.amax(leftEye[:, 1]), 0, self.full_img.shape[0] - 1)
leftEye_patch = cv2.resize(self.full_img[ymin:ymax, xmin:xmax],
(30, 10))
leftEye_patch = cv2.GaussianBlur(leftEye_patch, (9, 9), 0)
xmin = np.clip(np.amin(rightEye[:, 0]), 0, self.full_img.shape[1] - 1)
ymin = np.clip(np.amin(rightEye[:, 1]), 0, self.full_img.shape[0] - 1)
xmax = np.clip(np.amax(rightEye[:, 0]), 0, self.full_img.shape[1] - 1)
ymax = np.clip(np.amax(rightEye[:, 1]), 0, self.full_img.shape[0] - 1)
rightEye_patch = cv2.resize(self.full_img[ymin:ymax, xmin:xmax],
(30, 10))
rightEye_patch = cv2.GaussianBlur(rightEye_patch, (9, 9), 0)
sum_left = np.sum(leftEye_patch, axis=0)
sum_right = np.sum(rightEye_patch, axis=0)
eyeball_pos_left = np.argmin(sum_left)
eyeball_pos_right = np.argmin(sum_right)
cv2.line(leftEye_patch, (eyeball_pos_left, 0),
(eyeball_pos_left, rightEye_patch.shape[0]), (255), 1,
cv2.LINE_AA)
cv2.line(rightEye_patch, (eyeball_pos_right, 0),
(eyeball_pos_right, rightEye_patch.shape[0]), (255), 1,
cv2.LINE_AA)
gaze_angle_left = 5.0 * (leftEye_patch.shape[1] / 2 - eyeball_pos_left)
gaze_angle_right = 5.0 * (rightEye_patch.shape[1] / 2 -
eyeball_pos_right)
avg_gaze = (gaze_angle_right + gaze_angle_left) / 2
avg_gaze, _ = self.gaze_filter.update(avg_gaze)
return round(avg_gaze[0], 2), leftEye_patch, rightEye_patch
def mouth_status(self, shape):
(mStart, mEnd) = face_utils.FACIAL_LANDMARKS_IDXS["mouth"]
mouth = shape[mStart:mEnd]
mouthEAR = self.mouth_aspect_ratio(mouth)
if mouthEAR > 0.8:
return "yawn"
elif mouthEAR < 0.8 and mouthEAR > 0.4:
return "mouth open"
else:
return "mouth closed"
def brow_status(self, shape):
#classify as inner_brow_raise, outer_brow_raise, and brow lowerer. wrt to each other and wrt to upper eye
res = 0
normalizer = dist.euclidean(shape[24], shape[33]) + dist.euclidean(
shape[19], shape[33])
#inner brow raise
left_inner = dist.euclidean(shape[20], shape[38]) + dist.euclidean(
shape[21], shape[39])
right_inner = dist.euclidean(shape[22], shape[42]) + dist.euclidean(
shape[23], shape[43])
inner_brow_ratio = (left_inner + right_inner) / normalizer
#outer brow raise
left_outer = dist.euclidean(shape[18], shape[36]) + dist.euclidean(
shape[19], shape[37])
right_outer = dist.euclidean(shape[24], shape[44]) + dist.euclidean(
shape[25], shape[45])
outer_brow_ratio = (left_outer + right_outer) / normalizer
#brow lowerer
left_brow = shape[17:22]
_, _, _, _, std_err_left = stats.linregress(left_brow[:, 0],
left_brow[:, 1])
right_brow = shape[22:27]
_, _, _, _, std_err_right = stats.linregress(right_brow[:, 0],
right_brow[:, 1])
brow_lowerer_ratio = std_err_left + std_err_right
#thresholds = 0.50, 0.60, 0.18
res = 0
if inner_brow_ratio > 0.50:
res += 1
if outer_brow_ratio > 0.60:
res += 2
if brow_lowerer_ratio < 0.17:
res += 4
return res
def in_distress(self, shape):
small_triangle = dist.euclidean(shape[21], shape[27]) + dist.euclidean(
shape[22], shape[27]) + dist.euclidean(shape[21], shape[22])
large_triangle = dist.euclidean(shape[33], shape[26]) + dist.euclidean(
shape[26], shape[17]) + dist.euclidean(shape[17], shape[33])
procerus_aspect_ratio = small_triangle / large_triangle
if procerus_aspect_ratio < 0.190:
#in distress
return True
else:
return False
def mouth_aspect_ratio(self, mouth):
A = dist.euclidean(mouth[14], mouth[18])
B = dist.euclidean(mouth[3], mouth[9])
C = dist.euclidean(mouth[6], mouth[0])
# compute the eye aspect ratio
ear = (A + B) / (2.0 * C)
# return the eye aspect ratio
return ear
def eye_aspect_ratio(self, eye):
# compute the euclidean distances between the two sets of
# vertical eye landmarks (x, y)-coordinates
A = dist.euclidean(eye[1], eye[5])
B = dist.euclidean(eye[2], eye[4])
# compute the euclidean distance between the horizontal
# eye landmark (x, y)-coordinates
C = dist.euclidean(eye[0], eye[3])
# compute the eye aspect ratio
ear = (A + B) / (2.0 * C)
# return the eye aspect ratio
return ear
def classify_face(self, face_img):
face_img = cv2.resize(face_img, (48, 48))
try:
encoded_face = face_recognition.face_encodings(face_img)[0]
except:
return "unknown"
name = face_recognition_knn.predict(
[encoded_face], model_path="trained_knn_model.clf")[0]
if name != "unknown":
if name in self.names:
self.names[name] += 1
else:
self.names[name] = 1
name = max(self.names.items(), key=operator.itemgetter(1))[0]
return name