【人脸目标追踪】卡尔曼滤波原理解析与人脸追踪实例
追踪结果：
人脸追踪
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.结果
人脸追踪
参考：https://blog.csdn.net/u013453604/article/details/50301477