Exemplo n.º 1
0
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
Exemplo n.º 2
0
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()
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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
Exemplo n.º 7
0
    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')
Exemplo n.º 8
0
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
Exemplo n.º 9
0
    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 = []
Exemplo n.º 10
0
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()
Exemplo n.º 11
0
def test_1d_array():
    f1 = GHFilter(0, 0, 1, .8, .2)
    f2 = GHFilter(array([0]), array([0]), 1, .8, .2)

    # 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
    for i in range(1, 10):
        f1.update(i)
        f2.update(array([i]))

        assert f1.x == f2.x[0]
        assert f1.dx == f2.dx[0]

        assert f1.VRF() == f2.VRF()
Exemplo n.º 12
0
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()
####################################################

####################################################
# GH/Alpha betta filter - 2D attempt
####################################################

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()
Exemplo n.º 14
0
#!/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))










Exemplo n.º 15
0
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
Exemplo n.º 16
0
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