パーティクルフィルタ組んでみた(Python) - コンピュータサイエンス系勉強ノート

研究でトラッキングをする必要がでてきたので，トラッキング分野では非常によく用いられているパーティクルフィルタについて勉強しました．それについてまとめておこうと思います．

パーティクルフィルタとは?

時系列データ中の観測データがあるパラメータに従って確率的に生成されるとします．その観測値だけを元にパラメータを推定したいとします．パーティクルフィルタでは複数のパラメータ(パーティクルもしくは粒子とよばれる)を生成して，このパラメータが観測値を生成するのに相応しいか尤度を計算して有望なパラメータだけを残し，残ったパラメータの平均をとることで真のパラメータを推定します．最尤推定法などでは解析的に尤度の高いパラメータを推定しますが，パーティクルフィルタだと解析的に求められない場合でもパラメータを推定できます．

パーティクルフィルタの流れ

パーティクルフィルタは以下のような流れでパラメータの推定を行います．

パーティクル初期化
パーティクルの状態遷移
各パーティクルの重み(尤度)計算
重みの正規化
パーティクルのリサンプリング
期待値計算
２～６を繰り返す

まず①でパーティクルを初期化します．適当に玉をばらまいておくイメージです．次に②でパーティクルの状態を変化させます．パラメータは時間経過に伴ってなんらかの規則に従って変化するかもしれません．その規則をモデル化してこのパーティクルの状態遷移に組み込みます．例えば物体のトラッキングをしているときには時間経過に従って物体は移動します．等速で適当に動くかもしれません．その場合は適当な速度を位置パラメータに加えることになります．③の尤度計算では各パーティクルの尤度を計算します．④で重み(尤度)を正規化して⑤でパーティクルのリサンプリングを行います．大抵のリサンプリングでは重みにしたがって確率的にパーティクルを選びます．選ばれなかったパーティクルは捨てられることになります．最後に⑥の期待値計算を行います．リサンプリング後のパーティクルの平均値で計算できます．その後同じように②～⑥を繰り返すことになります．

パーティクルフィルタでトラッキングしてみる

パーティクルフィルタのpythonコードは以下の様になります．汚くてすいません．

#encoding:utf-8
import numpy as np
import cv2

def tracking():
    #カメラ
    cap = cv2.VideoCapture(0)
    #パーティクルフィルタ初期化
    filter = ParticleFilter()
    filter.initialize()

    while True:
        ret, frame = cap.read()
        gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
        y, x = filter.filtering(gray)#トラッキング
        frame = cv2.circle(frame, (int(x), int(y)), 10, (0, 0, 255), -1)
        for i in range(filter.SAMPLEMAX):
            frame = cv2.circle(frame, (int(filter.X[i]), int(filter.Y[i])), 2, (0, 0, 255), -1)
        cv2.imshow("frame", frame)
        if cv2.waitKey(20) & 0xFF == 27:
            break
    cap.release()
    cv2.destroyAllWindows()

#パーティクルフィルタクラス
class ParticleFilter:
    def __init__(self):
        self.SAMPLEMAX = 1000
        self.height, self.width = 480, 640#フレーム画像のサイズ

    #パーティクル初期化
    #画像全体にまく
    def initialize(self):
        self.Y = np.random.random(self.SAMPLEMAX) * self.height
        self.X = np.random.random(self.SAMPLEMAX) * self.width

    #パーティクルの状態の更新　物体が適当な速さで適当に動くと仮定
    def modeling(self):
        self.Y += np.random.random(self.SAMPLEMAX) * 20 - 10
        self.X += np.random.random(self.SAMPLEMAX) * 20 - 10

    #重み正規化
    def normalize(self, weight):
        return weight / np.sum(weight)

    #パーティクルリサンプリング
    #重みに従ってパーティクルを選択
    #残ったパーティクルのインデックスを返す
    def resampling(self, weight):
        index = np.arange(self.SAMPLEMAX)
        sample = []

        for i in range(self.SAMPLEMAX):
            idx = np.random.choice(index, p=weight)
            sample.append(idx)

        return sample

    #尤度計算
    #画像の外に飛んでいったパーティクルは重み0
    #白い物体を仮定
    def calcLikelihood(self, image):
        mean, std = 250.0, 10.0
        intensity = []

        for i in range(self.SAMPLEMAX):
            y, x = self.Y[i], self.X[i]
            if y >= 0 and y < self.height and x >= 0 and x < self.width:
                intensity.append(image[y,x])
            else:
                intensity.append(-1)

        weights = 1.0 / np.sqrt(2 * np.pi * std) * np.exp(-(np.array(intensity) - mean)**2 /(2 * std**2))
        weights[intensity == -1] = 0
        weights = self.normalize(weights)
        return weights

    #トラッキング開始
    #期待値を返す
    def filtering(self, image):
        self.modeling()
        weights = self.calcLikelihood(image)
        index = self.resampling(weights)
        self.Y = self.Y[index]
        self.X = self.X[index]
        
        return np.sum(self.Y) / float(len(self.Y)), np.sum(self.X) / float(len(self.X))

tracking()