Python 实现粒子滤波

 #转
 # -*- coding=utf-8 -*-
 # 直接运行代码可以看到跟踪效果
 # 红色的小点代表粒子位置
 # 蓝色的大点表示跟踪的结果
 # 白色的方框表示要跟踪的目标
 # 看懂下面两个函数即可
 from numpy import *
 from numpy.random import *

 def resample(weights):
     n = len(weights)
     indices = []
     # 求出离散累积密度函数(CDF)
     C = [0.] + [sum(weights[:i+1]) for i in range(n)]
     # 选定一个随机初始点
     u0, j = random(), 0
     for u in [(u0+i)/n for i in range(n)]: # u 线性增长到 1
         while u > C[j]: # 碰到小粒子，跳过
             j+=1
         indices.append(j-1)  # 碰到大粒子，添加，u 增大，还有第二次被添加的可能
     return indices # 返回大粒子的下标

 def particlefilter(sequence, pos, stepsize, n):
     ''' sequence: 表示图片序列
         pos： 第一帧目标位置
         stepsize： 采样范围
         n: 粒子数目
         '''
     seq = iter(sequence)
     x = ones((n, 2), int) * pos      # 100   ``aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa~!aa个初始位置(中心)
     f0 = seq.next()[tuple(pos)] * ones(n)  # 目标的颜色模型， 100 个 255
     yield pos, x, ones(n)/n # 返回第一帧期望位置(expectation pos)，粒子(x)和权重
     for im in seq:
         # 在上一帧的粒子周围撒点, 作为当前帧的粒子
         x += uniform(-stepsize, stepsize, x.shape)#uniform()随机生成函数
         # 去掉超出画面边框的粒子
         x  = x.clip(zeros(2), array(im.shape)-1).astype(int)
         f  = im[tuple(x.T)]  # 得到每个粒子的像素值
         w  = 1./(1. + (f0-f)**2)  # 求与目标模型的差异, w 是与粒子一一对应的权重向量
           # 可以看到像素值为 255 的权重最大(1.0)
         w /= sum(w)      # 归一化 w
         yield sum(x.T*w, axis=1), x, w # 返回目标期望位置，粒子和对应的权重
         if 1./sum(w**2) < n/2.:    # 如果当前帧粒子退化:
             x  = x[resample(w),:]  # 根据权重重采样, 有利于后续帧有效采样

 if __name__ == "__main__":
     from pylab import *
     from itertools import izip
     import time
     ion() # 打开交互模式
     seq = [ im for im in zeros((20,240,320), int)]      # 创建 20 帧全 0 图片
     x0 = array([120, 160])                              # 第一帧的框中心坐标

     # 为每张图片添加一个运动轨迹为 xs 的白色方块(像素值是255, 每帧横坐标加3,竖坐标加2)

     xs = vstack((arange(20)*3, arange(20)*2)).T + x0 # vstack: 竖直叠加
     for t, x in enumerate(xs):   # t 从 0 开始， x 从 xs[0] 开始，enumerate 函数用于遍历序列中的元素以及它们的下标
         # slice 的用法也很有意思，可以很方便用来表示被访问数组seq的下标范围
         xslice = slice(x[0]-8, x[0]+8)
         yslice = slice(x[1]-8, x[1]+8)
         seq[t][xslice, yslice] = 255

     # 跟踪白框
     for im, p in izip(seq, particlefilter(seq, x0, 8, 100)): #
         pos, xs, ws = p
         position_overlay = zeros_like(im)
         position_overlay[tuple(pos)] = 1
         particle_overlay = zeros_like(im)
         particle_overlay[tuple(xs.T)] = 1
         hold(True)
         draw()
         time.sleep(0.3)
         clf()                                           # Causes flickering, but without the spy plots aren't overwritten
         imshow(im,cmap=cm.gray)                         # Plot the image        spy(position_overlay, marker='.', color='b')    # Plot the expected position        spy(particle_overlay, marker=',', color='r')    # Plot the particles    show()