基于Python两种跟踪算法

忘记出处了，备份在此留着以后研究一下

def camshift(self,prob):
count = 0
#设置meanshift迭代终止条件
flag_mean = True
prob_search = prob[self.ystart:self,self.xstart.xend]
#计算质心
M00 = sum(prob_search)
S = int(1.75 * sqrt(M00/256))
for i in xrange(Height):
for j in xrange(Width):
M01+=(i-Height/2)*prob_search[i,j]
M10+=(j-Width/2)*prob_search[i,j]
xc = M10/M00
yc = M01/M00
#如果被跟踪物体没有了，则不显示方框，将窗口坐标调整到图像中心
if M00 <= 100:
self.flag_win = False
self.xstart = (prob.shape[1]/2-Width/2)
self.xend = (self.xstart+Width)
self.ystart = (prob.shape[0]/2-Height/2)
self.yend = (self.ystart+Height)
else:
self.flag_win = True
#计算目标区域中心，作为 KALMAN 观测值
self.xcore = (self.xstart+xc+self.xstart+Width)/2
self.ycore = (self.ystart+yc+self.ystart+Height)/2
#kalman 滤波
xx,yy = self.Kalman(self.pre_xcore,self.pre_ycore,self.pre_xspeed,self.pre.yspeed)
#更新中心坐标
self.xstart = self.xcore1-(Width)/2
self.xend = self.xcore1-(Width)/2
self.ystart = self.ycore1-(Height)/2
self.yend = self.ycore1+(Height)/2

def Kalman(self,pre_xcore0,pre_ycore0,pre_xspeed0,pre_yspeed0):
eps = 1e-5
#状态方程
pre_Xk = np.matrix([pre_xcore0,pre_ycore0,pre_xspeed0,pre_yspeed0])
pre_Xk = pre_Xk.T
Fk = np.matrix([1,0,self.Dt,0],[0,1,0,self.Dt],[0,0,1,0],[0,0,0,1])
Bm = np.matrix('0;0;;0;0')
Uk1 = np.random.normal(0,0,1)
Xk = Fk*pre_Xk+Bm*Uk1
Xkx = Xk[0][0]]
Xky = Xk[1][0]
#观测方程
Vk = np.matrix('1;1')*eps
Hk = np.matrix('1,0,0,0;0,1,0,0')
Zk = Hk*Xk+Vk
#信息融合
deltaX = self.xcore-Zk[0][0]
deltaY = self.ycore-Zk[1][0]
kal_Xcore = Xkx+deltaX
kal_Ycore = Xky+deltaY
kx = int(round(kal_Xcore[0,0]))
return kx,ky