【人脸目标跟踪】卡尔曼滤波原理分析及人脸跟踪实例

跟踪结果：

人脸追踪

1.卡尔曼滤波简介

卡尔曼滤波的一个典型例子是根据对象位置的有限（可能有偏差）观测值来预测对象位置的坐标和速度。它可以在许多工程应用中找到（例如雷达、计算机视觉）。同时，卡尔曼滤波器也是控制理论和控制系统工程中的重要课题。例如，对于雷达，人们对其跟踪物体的能力很感兴趣。但是对目标位置、速度和加速度的测量往往总是很嘈杂。卡尔曼滤波器利用目标的动态信息，试图去除噪声的影响，得到目标位置的良好估计。该估计可以是当前目标位置的估计（过滤）、未来位置的估计（预测）或过去位置的估计（插值或平滑）。

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2. 通俗理解

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好吧，现在对于给定的分钟，我们有两个房间温度值：您的经验预测值（系统的预测值）和温度计的值（测量值）。接下来我们用这两个值结合它们各自的噪声来估计房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

3.目标跟踪过程中的参数设置

4.人脸追踪实现

这里的人脸追踪先使用DLIB进行人脸检测，然后进行卡尔曼滤波器定义，参数设置。根据上面的介绍，我们知道需要设置参数状态矩阵，设置状态矩阵之后才能设置观测矩阵，因为观测矩阵的列维度需要状态矩阵确定，因为是目标检测，所以这里的状态矩阵不是上面的4维，我们将设置成6维，处理中心点，速度，还有目标框的宽高。然后观测矩阵也有2*4变成4*6，这里的6由状态矩阵决定，这里的4包括观测变量位置x,y，和目标宽高h，w。另外就是观测误差，预测误差，分别表示了观测和预测的可信度，误差越大，可信度越低。

import cv2
import numpy as np
import dlib

detector = dlib.get_frontal_face_detector()


def rect_to_bb(rect):
    x = rect.left()
    y = rect.top()
    w = rect.right() - x
    h = rect.bottom() - y
    return (x, y, w, h)
#状态向量
stateSize = 6
#观测向量
measSize = 4
coutrSize = 0
kf = cv2.KalmanFilter(stateSize,measSize,coutrSize)
state = np.zeros(stateSize, np.float32)#[x,y,v_x,v_y,w,h],簇心位置，速度，高宽
meas = np.zeros(measSize, np.float32)#[z_x,z_y,z_w,z_h]
procNoise = np.zeros(stateSize, np.float32)

#状态转移矩阵
cv2.setIdentity(kf.transitionMatrix)#生成单位矩阵
# [1 0 dT 0  0 0]
# [0 1 0  dT 0 0]
# [0 0 1  0  0 0]
# [0 0 0  1  0 0]
# [0 0 0  0  1 0]
# [0 0 0  0  0 1]
#观测矩阵
# [1 0 0 0 0 0]
# [0 1 0 0 0 0]
# [0 0 0 0 1 0]
# [0 0 0 0 0 1]
kf.measurementMatrix = np.zeros((measSize,stateSize),np.float32)
kf.measurementMatrix[0,0]=1.0
kf.measurementMatrix[1,1]=1.0
kf.measurementMatrix[2,4]=1.0
kf.measurementMatrix[3,5]=1.0

#预测噪声
# [Ex 0 0 0 0 0]
# [0 Ey 0 0 0 0]
# [0 0 Ev_x 0 0 0]
# [0 0 0 Ev_y 0 0]
# [0 0 0 0 Ew 0]
# [0 0 0 0 0 Eh]
cv2.setIdentity(kf.processNoiseCov)
kf.processNoiseCov[0,0] = 1e-2
kf.processNoiseCov[1,1] = 1e-2
kf.processNoiseCov[2,2] = 5.0
kf.processNoiseCov[3,3] = 5.0
kf.processNoiseCov[4,4] = 1e-2
kf.processNoiseCov[5,5] = 1e-2

#测量噪声
cv2.setIdentity(kf.measurementNoiseCov)
# for i in range(len(kf.measurementNoiseCov)):
#     kf.measurementNoiseCov[i,i] = 1e-1


video_cap = cv2.VideoCapture(0)
# 视频输出
fps = video_cap.get(cv2.CAP_PROP_FPS) #获得视频帧率，即每秒多少帧
size = (int(video_cap.get(cv2.CAP_PROP_FRAME_WIDTH)),int(video_cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
videoWriter = cv2.VideoWriter('./video/new_green.mp4' ,cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), fps, size)
ticks = 0 
i=0
found = False
notFoundCount = 0
prePointCen = [] #存储小球中心点位置
meaPointCen = []
while(True):
    ret, frame = video_cap.read()
    if ret is False:
        break
#    cv2.imshow('frame',frame)
#    cv2.waitKey(1)
    precTick = ticks  
    ticks = float(cv2.getTickCount())  
    res = frame.copy()
    # dT = float(1/fps)  
    dT = float((ticks - precTick)/cv2.getTickFrequency()) 
    if(found):
        #预测得到的小球位置
        kf.transitionMatrix[0,2] = dT
        kf.transitionMatrix[1,3] = dT

        state = kf.predict()
        width = state[4]
        height = state[5]
        x_left = int(state[0] - width/2) #左上角横坐标
        y_left = int(state[1] - height/2)  #左上角纵坐标
        x_right = int(state[0] + width/2)
        y_right = int(state[1] + height/2)

        center_x = state[0]
        center_y = state[1]
        prePointCen.append((int(center_x),int(center_y)))
        cv2.circle(res, (int(center_x),int(center_y)),2,(255,0,0),-1)
        cv2.rectangle(res,(x_left,y_left),(x_right,y_right),(255,0,0),2)


    # 取灰度
    img_gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)

	# 人脸数rects
	balls = []
	ballsBox = []
	rects = detector(img_gray, 0)

    if len(rects)>0:
        x, y, w, h = rect_to_bb(rects[0])
        balls.append(i)
        ballsBox.append([x, y, w, h])


    print( "Balls found:", len(ballsBox))
    print("\n")

    for i in range(len(balls)):
        # 绘制小球轮廓
        cv2.rectangle(res,(ballsBox[i][0],ballsBox[i][1]),(ballsBox[i][0]+ballsBox[i][2],ballsBox[i][1]+ballsBox[i][3]),(0,255,0),2) #二值化得到小球边界

        center_x = ballsBox[i][0] + ballsBox[i][2] / 2
        center_y = ballsBox[i][1] + ballsBox[i][3] / 2

        meaPointCen.append((int(center_x),int(center_y)))
        cv2.circle(res,(int(center_x),int(center_y)), 2, (20,150,20) ,-1)

        name = "(" + str(center_x) + "," + str(center_y) + ")"
        cv2.putText(res, name, (int(center_x) + 3, int(center_y) - 3), cv2.FONT_HERSHEY_COMPLEX, 0.5, (20,150,20), 2)
    n = len(prePointCen)
    for i in range(1, n):
        print(i)
        if prePointCen[i-1] is None or prePointCen[i] is None:
            continue

        cv2.line(res, prePointCen[i-1], prePointCen[i], (0,0,255), 1, 4)
    if(len(balls) == 0):
        notFoundCount += 1
        print("notFoundCount",notFoundCount)
        print("\n")

        if notFoundCount >= 100:
            found = False

    else:
        #测量得到的物体位置
        notFoundCount = 0
        meas[0] = ballsBox[0][0] + ballsBox[0][2] / 2
        meas[1] = ballsBox[0][1] + ballsBox[0][3] / 2
        meas[2] = float(ballsBox[0][2])
        meas[3] = float(ballsBox[0][3])

        #第一次检测
        if not found:
            for i in range(len(kf.errorCovPre)):
                kf.errorCovPre[i,i] = 1
            state[0] = meas[0]
            state[1] = meas[1]
            state[2] = 0
            state[3] = 0
            state[4] = meas[2]
            state[5] = meas[3]

            kf.statePost = state
            found = True

        else:
            kf.correct(meas) #Kalman修正

            print('rr',res.shape)
            print("Measure matrix:", meas)
    cv2.imshow("Tracking", res)
    cv2.waitKey(5)

5.结果