パーティクルフィルタによる物体追跡

今回はパーティクルフィルタを簡単に紹介。
(Wikipedia: 粒子フィルタ – Wikipedia)

これは、一般状態空間モデルにおける状態ベクトルの推定法で、
Wikipediaではなにやら難しげに書かれているように見えますが、
要は、条件付き分布をたくさんのサンプル点で近似表現するだけの手法です。
この手法は、逐次モンテカルロ法とも呼ばれているように、
ランダムサンプリングによるモンテカルロ近似によって状態推定を行います。

パーティクルフィルタを物体追跡に適用するためには、

・システムモデル（状態遷移関数）
・観測モデル（尤度関数）

の2つを設計する必要があります。
今回は状態遷移に線形予測モデル、つまり等速直線運動を仮定し、
尤度(ゆうど：もっともらしさ)は “赤色らしさ” とします。
この尤度関数の設計はOpenCVのサンプルコードからお借りしました。感謝。

wonderflにサンプルを投稿しておきましたので興味があれば。
サンプル集合の初期状態があまり良くないと追跡に失敗する場合があるので、
その時はリロードするか、動画中盤あたりまでしばらく様子を見てください。

Particle Filter – wonderfl build flash online

以下、Actionscriptでの例です。

・クライアント
(サンプル集合の重心位置だけでなく、サンプル点も全て表示しています)

package {
  import flash.display.Bitmap;
  import flash.display.BitmapData;
  import flash.display.Shape;
  import flash.display.Sprite;
  import flash.events.Event;
  import flash.events.NetStatusEvent;
  import flash.media.Video;
  import flash.net.NetConnection;
  import flash.net.NetStream;

  public class Main extends Sprite {
    private var screen:Bitmap;
    private var upper:Vector.<int>;
    private var lower:Vector.<int>;
    private var noise:Vector.<int>;
    private var num:int;
    private var tracker:ParticleFilter;
    private var point:Shape;
    private var source:String = "test.f4v";
    private var connection:NetConnection;
    private var stream:NetStream;
    private var video:Video;
    
    public function Main():void {
      stage.scaleMode = "noScale";
      screen = new Bitmap(new BitmapData(465, 348, false, 0));
      addChild(screen);
      var w:int = screen.width;
      var h:int = screen.height;
      upper = new <int>[w, h, 10, 10];  // upper boundary for each dimension
      lower = new <int>[0, 0, -10, -10];  // lower .
      noise = new <int>[30, 30, 10, 10];  // noise .
      num = 300;  // number of particle
      tracker = new ParticleFilter(num, w, h, upper, lower, noise);
      
      point = new Shape();
      point.graphics.beginFill(0x0099ff);
      point.graphics.drawCircle(0, 0, 5);
      point.graphics.endFill();
      addChild(point);

      // show initial sample set (particles)
      var data:Vector.<uint> = screen.bitmapData.getVector(screen.bitmapData.rect);
      for(var i:int=0; i<num; i++) {
        var x:int = tracker.samples[i].x[0];
        var y:int = tracker.samples[i].x[1];
        data[y*w + x] = 0xffffff;
      }
      screen.bitmapData.setVector(screen.bitmapData.rect, data);
      
      connection = new NetConnection();
      connection.addEventListener(NetStatusEvent.NET_STATUS, checkConnect);
      connection.connect(null);      
    }
    private function checkConnect(e:NetStatusEvent):void {
      removeEventListener(NetStatusEvent.NET_STATUS, arguments.callee);
      if(e.info.code == "NetConnection.Connect.Success") {
        stream = new NetStream(connection);
        stream.client = {};
        video = new Video(screen.width, screen.height);
        video.attachNetStream(stream);
        stream.play(source);
      }else {
        trace(e.info.code);
      }
    }
    private function update(e:Event):void {
      try {
        var bmpData:BitmapData = screen.bitmapData;
        bmpData.draw(video);
        var data:Vector.<uint> = bmpData.getVector(bmpData.rect);
        // apply particle filter
        tracker.resample();
        tracker.predict();
        tracker.weight(data);
        // show result (weighted mean)
        var result:Particle = tracker.measure();
        point.x = result.x[0];
        point.y = result.x[1];
        // show particles
        for(var i:int=0; i<num; i++) {
          var x:int = tracker.samples[i].x[0];
          var y:int = tracker.samples[i].x[1];
          data[y*screen.width + x] = 0xffffff;
        }
        bmpData.setVector(bmpData.rect, data);
      }catch(e:RangeError) {
        // ignore
      }
    }
  }
}

package {

import flash.display.Bitmap;

import flash.display.BitmapData;

import flash.display.Shape;

import flash.display.Sprite;

import flash.events.Event;

import flash.events.NetStatusEvent;

import flash.media.Video;

import flash.net.NetConnection;

import flash.net.NetStream;

public class Main extends Sprite {

private var screen:Bitmap;

private var upper:Vector.<int>;

private var lower:Vector.<int>;

private var noise:Vector.<int>;

private var num:int;

private var tracker:ParticleFilter;

private var point:Shape;

private var source:String = "test.f4v";

private var connection:NetConnection;

private var stream:NetStream;

private var video:Video;

public function Main():void {

stage.scaleMode = "noScale";

screen = new Bitmap(new BitmapData(465, 348, false, 0));

addChild(screen);

var w:int = screen.width;

var h:int = screen.height;

upper = new <int>[w, h, 10, 10]; // upper boundary for each dimension

lower = new <int>[0, 0, -10, -10]; // lower .

noise = new <int>[30, 30, 10, 10]; // noise .

num = 300; // number of particle

tracker = new ParticleFilter(num, w, h, upper, lower, noise);

point = new Shape();

point.graphics.beginFill(0x0099ff);

point.graphics.drawCircle(0, 0, 5);

point.graphics.endFill();

addChild(point);

// show initial sample set (particles)

var data:Vector.<uint> = screen.bitmapData.getVector(screen.bitmapData.rect);

for(var i:int=0; i<num; i++) {

var x:int = tracker.samples[i].x[0];

var y:int = tracker.samples[i].x[1];

data[y*w + x] = 0xffffff;

}

screen.bitmapData.setVector(screen.bitmapData.rect, data);

connection = new NetConnection();

connection.addEventListener(NetStatusEvent.NET_STATUS, checkConnect);

connection.connect(null);

}

private function checkConnect(e:NetStatusEvent):void {

removeEventListener(NetStatusEvent.NET_STATUS, arguments.callee);

if(e.info.code == "NetConnection.Connect.Success") {

stream = new NetStream(connection);

stream.client = {};

video = new Video(screen.width, screen.height);

video.attachNetStream(stream);

stream.play(source);

}else {

trace(e.info.code);

}

private function update(e:Event):void {

try {

var bmpData:BitmapData = screen.bitmapData;

bmpData.draw(video);

var data:Vector.<uint> = bmpData.getVector(bmpData.rect);

// apply particle filter

tracker.resample();

tracker.predict();

tracker.weight(data);

// show result (weighted mean)

var result:Particle = tracker.measure();

point.x = result.x[0];

point.y = result.x[1];

// show particles

for(var i:int=0; i<num; i++) {

var x:int = tracker.samples[i].x[0];

var y:int = tracker.samples[i].x[1];

data[y*screen.width + x] = 0xffffff;

}

bmpData.setVector(bmpData.rect, data);

}catch(e:RangeError) {

// ignore

}

・ParticleFilter.as (パーティクルフィルタ)

package  {
  public class ParticleFilter {
    private var dim:int;
    private var num:int;
    private var w:int;
    private var h:int;
    private var upper:Vector.<int>;
    private var lower:Vector.<int>;
    private var noise:Vector.<int>;
    public var samples:Vector.<Particle>;
    
    public function ParticleFilter(num:int, w:int, h:int,
                                   upper:Vector.<int>,
                                   lower:Vector.<int>,
                                   noise:Vector.<int>) {
      this.dim = 4;                             
      this.num = num;
      this.w = w;
      this.h = h;
      this.upper = upper;
      this.lower = lower;
      this.noise = noise;
      this.samples = new Vector.<Particle>(num, true);
      // initializes the sample set.
      for(var i:int=0; i<num; i++) {
        var p:Particle = new Particle();
        for(var j:int=0; j<dim; j++) {
          var r:int = int(Math.random()*32767);
          p.x[j] = r%(upper[j] - lower[j]) + lower[j];
        }
        p.w = 1.0/num;
        samples[i] = p;
      }
    }
    /* returns the weighted mean as estimated result. */
    public function measure():Particle {
      var result:Particle = new Particle();
      var x:Vector.<Number> = new Vector.<Number>(dim, true);
      for(var i:int=0; i<num; i++) {
        x[0] += samples[i].x[0]*samples[i].w;
        x[1] += samples[i].x[1]*samples[i].w;
        x[2] += samples[i].x[2]*samples[i].w;
        x[3] += samples[i].x[3]*samples[i].w;        
      }
      for(var k:int=0; k<dim; k++) {
        result.x[k] = int(x[k]);
      }
      return result;
    }
    /**
     * estimates the subsequent model state,
     * based on linear uniform motion.
     */
    public function predict():void {
      for(var i:int=0; i<num; i++) {
        // update random noise
        var n:Vector.<int> = new Vector.<int>(dim, true);
        var max:Number = 32367;
        n[0] = int(Math.random()*max%(noise[0]*2) - noise[0]);
        n[1] = int(Math.random()*max%(noise[1]*2) - noise[1]);
        n[2] = int(Math.random()*max%(noise[2]*2) - noise[2]);
        n[3] = int(Math.random()*max%(noise[3]*2) - noise[3]);
        // update state
        var v:Vector.<int> = samples[i].x;
        v[0] += v[2] + n[0];
        v[1] += v[3] + n[1];
        v[2] = n[2];
        v[3] = n[3];
        if(v[0] < lower[0]) v[0] = lower[0];
        if(v[1] < lower[1]) v[1] = lower[1];
        if(v[2] < lower[2]) v[2] = lower[2];
        if(v[3] < lower[3]) v[3] = lower[3];
        if(v[0] >= upper[0]) v[0] = upper[0];
        if(v[1] >= upper[1]) v[1] = upper[1];
        if(v[2] >= upper[2]) v[2] = upper[2];
        if(v[3] >= upper[3]) v[3] = upper[3];
      }
    }
    /* resampling based on sample's weight. */
    public function resample():void {
      // accumulate weight
      var w:Vector.<Number> = new Vector.<Number>(num, true);
      w[0] = samples[0].w;
      for(var i:int=1; i<num; i++) {
        w[i] = w[i - 1] + samples[i].w;
      }
      var pre:Vector.<Particle> = Vector.<Particle>(samples);
      for(var j:int=0; j<num; j++) {
        var darts:Number = (Math.random()*32767%10000)/10000.0;
        for(var k:int=0; k<num; k++) {
          if(darts>w[k]) {
            continue;
          }else {
            // resampling
            samples[j].x[0] = pre[k].x[0];  // x
            samples[j].x[1] = pre[k].x[1];  // y
            samples[j].x[2] = pre[k].x[2];  // u
            samples[j].x[3] = pre[k].x[3];  // v
            samples[j].w = 0.0;
            break;
          }
        }
      }
    }
    /* calculates the likelifood for each sample (red color). */
    public function weight(img:Vector.<uint>):void {
      var sum:Number = 0.0;
      for(var i:int=0; i<num; i++) {
        var x:int = samples[i].x[0];
        var y:int = samples[i].x[1];
        var pos:int = y*w + x;
        if(pos < w*h) {
          samples[i].w = likelifood(img[pos]);
        }else {
          samples[i].w = 0.0001;
        }
        sum += samples[i].w;
      }
      // normalize
      for(var j:int=0; j<num; j++) {
        samples[j].w /= sum;
      }
    }
    private function likelifood(value:uint):Number {
      var sigma:Number = 50.0;
      var r:uint = value >> 16 & 0xff;
      var g:uint = value >> 8 & 0xff;
      var b:uint = value & 0xff;
      var dist:Number = Math.sqrt(b*b + g*g + (255 - r)*(255 - r));
      return 1.0/(Math.sqrt(2.0*Math.PI)*sigma)*Math.exp(-dist*dist/(2.0*sigma*sigma));
    }
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

package {

public class ParticleFilter {

private var dim:int;

private var num:int;

private var w:int;

private var h:int;

private var upper:Vector.<int>;

private var lower:Vector.<int>;

private var noise:Vector.<int>;

public var samples:Vector.<Particle>;

public function ParticleFilter(num:int, w:int, h:int,

upper:Vector.<int>,

lower:Vector.<int>,

noise:Vector.<int>) {

this.dim = 4;

this.num = num;

this.w = w;

this.h = h;

this.upper = upper;

this.lower = lower;

this.noise = noise;

this.samples = new Vector.<Particle>(num, true);

// initializes the sample set.

for(var i:int=0; i<num; i++) {

var p:Particle = new Particle();

for(var j:int=0; j<dim; j++) {

var r:int = int(Math.random()*32767);

p.x[j] = r%(upper[j] - lower[j]) + lower[j];

}

p.w = 1.0/num;

samples[i] = p;

}

/* returns the weighted mean as estimated result. */

public function measure():Particle {

var result:Particle = new Particle();

var x:Vector.<Number> = new Vector.<Number>(dim, true);

for(var i:int=0; i<num; i++) {

x[0] += samples[i].x[0]*samples[i].w;

x[1] += samples[i].x[1]*samples[i].w;

x[2] += samples[i].x[2]*samples[i].w;

x[3] += samples[i].x[3]*samples[i].w;

}

for(var k:int=0; k<dim; k++) {

result.x[k] = int(x[k]);

}

return result;

}

/**

* estimates the subsequent model state,

* based on linear uniform motion.

public function predict():void {

for(var i:int=0; i<num; i++) {

// update random noise

var n:Vector.<int> = new Vector.<int>(dim, true);

var max:Number = 32367;

n[0] = int(Math.random()*max%(noise[0]*2) - noise[0]);

n[1] = int(Math.random()*max%(noise[1]*2) - noise[1]);

n[2] = int(Math.random()*max%(noise[2]*2) - noise[2]);

n[3] = int(Math.random()*max%(noise[3]*2) - noise[3]);

// update state

var v:Vector.<int> = samples[i].x;

v[0] += v[2] + n[0];

v[1] += v[3] + n[1];

v[2] = n[2];

v[3] = n[3];

if(v[0] < lower[0]) v[0] = lower[0];

if(v[1] < lower[1]) v[1] = lower[1];

if(v[2] < lower[2]) v[2] = lower[2];

if(v[3] < lower[3]) v[3] = lower[3];

if(v[0] >= upper[0]) v[0] = upper[0];

if(v[1] >= upper[1]) v[1] = upper[1];

if(v[2] >= upper[2]) v[2] = upper[2];

if(v[3] >= upper[3]) v[3] = upper[3];

}

/* resampling based on sample's weight. */

public function resample():void {

// accumulate weight

var w:Vector.<Number> = new Vector.<Number>(num, true);

w[0] = samples[0].w;

for(var i:int=1; i<num; i++) {

w[i] = w[i - 1] + samples[i].w;

}

var pre:Vector.<Particle> = Vector.<Particle>(samples);

for(var j:int=0; j<num; j++) {

var darts:Number = (Math.random()*32767%10000)/10000.0;

for(var k:int=0; k<num; k++) {

if(darts>w[k]) {

continue;

}else {

// resampling

samples[j].x[0] = pre[k].x[0]; // x

samples[j].x[1] = pre[k].x[1]; // y

samples[j].x[2] = pre[k].x[2]; // u

samples[j].x[3] = pre[k].x[3]; // v

samples[j].w = 0.0;

break;

}

/* calculates the likelifood for each sample (red color). */

public function weight(img:Vector.<uint>):void {

var sum:Number = 0.0;

for(var i:int=0; i<num; i++) {

var x:int = samples[i].x[0];

var y:int = samples[i].x[1];

var pos:int = y*w + x;

if(pos < w*h) {

samples[i].w = likelifood(img[pos]);

}else {

samples[i].w = 0.0001;

}

sum += samples[i].w;

}

// normalize

for(var j:int=0; j<num; j++) {

samples[j].w /= sum;

}

private function likelifood(value:uint):Number {

var sigma:Number = 50.0;

var r:uint = value >> 16 & 0xff;

var g:uint = value >> 8 & 0xff;

var b:uint = value & 0xff;

var dist:Number = Math.sqrt(b*b + g*g + (255 - r)*(255 - r));

return 1.0/(Math.sqrt(2.0*Math.PI)*sigma)*Math.exp(-dist*dist/(2.0*sigma*sigma));

}

・Particle.as (サンプル点)

package  {
  public class Particle {
    public var x:Vector.<int>; // vector of state (x, y, u, v)
    public var w:Number; // weight
    
    public function Particle() {
      x = new Vector.<int>(4, true);
      w = 0.0;
    }
  }
}

package {

public class Particle {

public var x:Vector.<int>; // vector of state (x, y, u, v)

public var w:Number; // weight

public function Particle() {

x = new Vector.<int>(4, true);

w = 0.0;

}

4次元なので手動でインライン展開してる所が多く見難いですが。。

Flashの馬力だと、サンプル数 1000 以上はちょっと厳しそうです。
また、上述のコードでは尤度計算をRGB表色系のまま行っていますが、
実際はHSV表色系で計算した方がより照明変化にロバストな結果が得られます。

パーティクルフィルタはパラメトリックな状態ベクトルで表現可能なモデルであれば、
基本的にはどんな対象でもOKで、観測モデルも尤度関数さえ定義できればいいので、
(実際は尤度関数の設計に頭を悩ませることになるかと；)
コンピュータビジョンの分野(主に物体追跡)でも多くの適用例があります。
トラッキングはそれなりに専門で研究していたのですが、
結局、パーティクルフィルタは軽い検証だけして後はまったく触れず仕舞いでした。
こういった確率モデルを画像処理に適用する手法は興味深いので、改めて学び直したいです。

・参考
CiNii 論文 – 粒子フィルタ
 Tutorial/Practice0 MIST Project
opencv.jp – OpenCV: 推定器（Estimators）サンプルコード –

パーティクルフィルタによる物体追跡

あわせて読む:

One thought

コメントを残す