Scipy Cookbookの粒子フィルタ・パーティクルフィルタ・粒子モンテカルロ・モンテカルロフィルタ（別名あり過ぎだろ…）のサンプルを写経してみた
Scipyにあるcookbookの粒子フィルタの実装を主にpythonのリハビリを兼ねて行った。基本になっているのはCookbook / ParticleFilterのコード。
どうも上のサイトのコードをそのまま動かそうとすると「物体を追跡してるぞ！」感がなくなるのでうまくアニメーションするようにコードは書き換えてある。あとリハビリ用コメント追記・関数化も実施。
# -*- coding: utf-8 -*- #「物体の軌道を粒子フィルタで追跡している」という問題設定。 #白が物体で青がパーティクルから推定される加重平均された重心で赤が各々のパーティクル import numpy.random, numpy #物体の重心の軌跡を返す関数 def create_trajectory(size_z, init_pos): #arange(20) = [0,1,2,3,.....19]のnumpyの配列 #vstackという関数はarrayをまとめて次元をあげて返す関数。同様にhstackもある。 #名前はhorizon とverticalの略か？vstack自体はRでいうcbind的な挙動 #これだと(0,0)→(1,1)→(2,2)…みたいな感じで軌跡が返る path = numpy.vstack((numpy.arange(size_z), numpy.arange(size_z))).T + init_pos return path #分析対象となる画像を生成する関数 def create_images(size_image, size_object, init_pos): #size_x * size_yの2次元arrayをsize_z個持つlistとして作る。 #画像がｚ方向に串刺さっているイメージ。そしてこれを軌跡の画像とみなす size_x, size_y, size_z = size_image result_image = [pixel_image for pixel_image in numpy.zeros((size_z,size_x,size_y), int)] #物体の軌道作成。具体的な軌道はcreate_trajectory関数内で指定 trajectory = create_trajectory(size_z, init_pos) #enumerateで軌道のインデックスとその座標でループ for index_trajectory, position in enumerate(trajectory): #sliceオブジェクトは配列のインデックス付けに使えるオブジェクト #Rでいうベクトルの要素をとるときのx[4:10]みたいな書き方のpython版 xslice = slice(position[0]-size_object, position[0]+size_object) yslice = slice(position[1]-size_object, position[1]+size_object) #動いてる様子の記述.物体のある（と思われる箇所）を白塗りしてるイメージ result_image[index_trajectory][xslice, yslice] = 255 return result_image #weighsという重みでリサンプリング #返却値はn個の要素を持つインデックス！！！ def resample(weights): n = len(weights) indices = [] #経験累積分布作成 C = [0.] + [sum(weights[:i+1]) for i in range(n)] u0, j = numpy.random.random(), 0 for u in [(u0+i)/n for i in range(n)]: while u > C[j]: j+=1 indices.append(j-1) return indices #images:複数枚の画像（だと思っているもの） #pos：各jパーティクルの初期座標 #stepsize：パーティクルを動かすための幅 #n:バラ撒く粒子の個数100 def particlefilter(images, pos, stepsize, n): #画像のiterator取得。next関数で画像を初めから一枚ずつ取得していく感じ。 iterator_images = iter(images) # particleの初期位置の設定 #2個要素あるnumpy.arrayがn個あるnumpy.array。粒子の位置ベクトル position_particles = numpy.ones((n, 2), int) * pos #一番はじめの画像のposの位置のピクセル値を取得してn個用意(array取得） f0 = iterator_images.next()[tuple(pos)] * numpy.ones(n) #一番はじめの予測位置、パーティクルの位置、ウェイトを返却 yield pos, position_particles, numpy.ones(n)/n #二枚目以降でループ。生の画像本体でまわすのとiteratorで回すのはあまり関係ないっぽい for image in iterator_images: # 観測モデル: -stepsizeからstepsizeまでの一様乱数をposition_particles.shapeの形に合うように１こずつ生成。 #要するにxの要素が-stepsizeからstepsizeまでの一様乱数に置き換わったもんが出てくる #んで、position_particlesに加算されると position_particles += numpy.random.uniform(-stepsize, stepsize, position_particles.shape) #値に上下限を設定。下限0・上限画像サイズの縦と横。 #一応、ピクセル上の位置だと思っているのでintに無理やりキャスト position_particles = position_particles.clip(numpy.zeros(2), numpy.array(image.shape)-1).astype(int) #動かしたパーティクルの位置の色（白か黒か）を取得 f = image[tuple(position_particles.T)] #ウェイトを以下のように定義。尤度をこうおいてると思えってことか。 #f0使い続けるのは怪しい weights = 1./(1. + (f0-f)**2) #規格化 weights /= numpy.sum(weights) #予測位置、各パーティクルの位置、ウェイトを返却 yield numpy.sum(position_particles.T*weights, axis=1), position_particles, weights #個数が適当な閾値以下だったらリサンプリングするらしい if 1./numpy.sum(weights**2) < n/2.: position_particles = position_particles[resample(weights),:] if __name__ == '__main__': import pylab, itertools, time #matplotlibの関数。inteactive mode を on にする。 #公式サイトでの説明もこんだけ。 pylab.ion() #追跡用画像作成。サイズは適当に。 init_pos = (120,160) images = create_images((320,320,20), 15, init_pos) #izipはzip関数のiteration返す版 #zip関数は二つのリストを各リストの要素ごとにタプルにして固めたリストを作る。 #imにseq,pにparticlefilter関数からの値が逐次返ってきている for image, p in itertools.izip(images, particlefilter(images, init_pos, 8, 100)): #予測位置、パーティクル、ウェイトを返却 position_center, position_particles, weights_particle = p #予測フィルタからの結果を取得 #zeros_likeは同じサイズの零行列（配列）を戻す関数 position_overlay = numpy.zeros_like(image) position_overlay[tuple(position_center)] = 1 #バラ撒いたparticleの場所を取得 particle_overlay = numpy.zeros_like(image) particle_overlay[tuple(position_particles.T)] = 1 #上書きしていかないで追記する pylab.hold(False) #強制再描画。ファイルに吐くときのflash的なもんか？公式サイトにも１行しか説明なし pylab.draw() time.sleep(1) #画像（image）を指定したカラーマップ（cm.gray）で書く #help(cm)とかすれば他のカラーマップにどういうのがあるのか情報取れる pylab.imshow(image, cmap = pylab.cm.gray) #予測フィルタから出てきた物体の位置を青で書く #markerには's’:四角(default)、'o’:円、‘.’：点、‘,’ ：ピクセルが指定可能 pylab.spy(position_overlay, marker='.', color='b') #バラ撒いたパーティクルを赤く書く pylab.spy(particle_overlay, marker=',', color='r') pylab.show()
実行すると白い移動物体の重心位置を赤いパーティクルで推定（青）するような形のアニメーションが実施される。
ただ、このscipy/cookbookにあるサンプルコード間違ってるんじゃないかなと思うので要検証。
特に尤度関数（weight）の設定がおかしい気がする。→いろいろいじったけどまだわからん。
・・・ただ、まぁpythonのリハビリという点ではいい練習になった。
【参考リンク】
モンテカルロフィルター（粒子フィルター）のソースコード・入門書
Cookbook / ParticleFilter
粒子フィルタ（wikipedia)
