python + numpyで緑色の物体を追跡する (パーティクルフィルタ)

##初めに
以前、このような記事(pythonで赤い物体を認識しよう)を書きました。
単純に、画像をHSV(Hue, Saturation, Value)に変換し、赤成分の強い領域を見つけ出すという手法です。

今回は、その応用として「緑色」の物体について考えてみます。そしてさらに、連続したビデオストリームで物体を「追跡する」という事について記載してみようと思います。
連続したビデオストリームも細かく見れば連続した静止画です。この静止画から緑色の物体を認識し続ければ、自ずと追跡も出来るのでは？と考えるのも、大きく間違っていません。ただ、そこは人間の目と違うカメラ。ターゲットの物体の角度が少しズレれば光の加減で白光りして判定から逸れてしまったり(判定失敗して見失う)、判定箇所がアチコチ飛び回ったり、環境光が少し変わるだけで見失ったりするものなのです。
たまたま悪い条件が重なり見失ってしまうとどうなるでしょうか。コンピュータはその物体を「無い」ものと判定してしまいます。でも、その次の瞬間には条件が好転しまた現れるかも知れません。現れては消え、また現れたり、全く安定感がありません、常に100%の検出精度は期待できないのです。

従って、ビデオストリームであっても静止画を都度解析すれば良いという考えは捨て、ビデオストリームを「連続した静止画」と考えを改める必要があります。つまり「前回ココにあったから今回もこの辺りにあるはずだ」というのを、確率と統計の考えを駆使して予測して探索を掛けるべきなのです。

##パーティクルフィルタ
こういう問題を解決するのに良い手法があります。「パーティクルフィルタ」というものです。パーティクルフィルタの学問的な定義や内容については、自分も的確に説明できるほど判ってないので数あるその他の解説ページにお任せしたいですｗ
ただ、パーティクルフィルタによる画像解析の概念的には、

1. パーティクル(点)を画像上にばら撒く
2. そのパーティクル毎に周辺の尤度(ゆうど：重み。例えば赤っぽさとか、青っぽさとか)を算出する
3. 尤度の高いパーティクルを生かし、低いパーティクルは除去する
4. 尤度の高いパーティクルが最も集中している点を見つける (これが物体の位置となる)
5. すべてのパーティクルをランダムに少しづつ動かして次のフレームに備える (→2.へ)

これら、1～5の手続きを連続させれば、自ずと上記に記載したことが実現できてしまいますね。
「前回ここら辺にあった」という状態はばら撒かれたパーティクルが覚えており、そのパーティクル毎に「次はこの辺かも？」と、パーティクルをランダムに動くということを繰り返します。予測をハズし続ける出来の悪いパーティクルは淘汰されます。パーティクルは必死に次のフレームを予測し、生き延びるのですｗ 意外と残酷な感じもして、パーティクルも大変だな、と愛着すら湧いてきそうです(ウソ)。

これら一連の処理をC/C++で記載するとけっこう大変なんですが、主にnumpyの威力により、少ないコード量で済むのがpython + numpyの凄いトコロです。

##パーティクルフィルタの実装 (基本)

一応ですが、最初にimportについて。
以下のようにimportしておくのを前提としておきます。

import cv2
import numpy as np

###パーティクルの定義

まずはパーティクルの構造を定義します。以下のようにしましょう。x, yはパーティクルの位置、weightは尤度(重み)とします。

particle = [x, y, weight]

###尤度算出関数

で、次にパーティクルの尤度(重み)を算出する関数を作ります。
この関数は、指定された座標を中心に30x30ピクセルの範囲を走査し、900ピクセルのうち、条件に合致するピクセル数の割合を返します。例えば30x30ピクセルの範囲が全てNGなら0.0, 全てOKなら1.0, 半分くらいがOKなら0.5程度の値が返ります。
判定関数func()は外出ししていて、呼び出し側が判定関数を指定できるようにします。

def likelihood(x, y, func, image, w=30, h=30): 
  x1 = max(0, x - w / 2)
  y1 = max(0, y - h / 2)
  x2 = min(image.shape[1], x + w / 2)
  y2 = min(image.shape[0], y + h / 2)
  region = image[y1:y2, x1:x2]
  count = region[func(region)].size
  return (float(count) / image.size) if count > 0 else 0.0001

###パーティクルの初期化関数

次は、パーティクルの初期化関数です。
funcパラメータは先ほどの同様に、外出しされる判定関数を指定するのを想定しています。
この関数では、このパーティクルをたくさん(500個)作ります。
初期値としては、func()関数で判定される領域のうち、最も大きな領域付近にパーティクルを位置付けます。
np.ndarray()の呼び出しに指定している(500, 3)の500はパーティクルの個数、3は上記のx, y, weightの要素数です。

def init_particles(func, image): 
  mask = image.copy()
  mask[func(mask) == False] = 0
  contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
  if len(contours) <= 0:
    return None
  max_contour = max(contours, key=cv2.contourArea)
  max_rect = np.array(cv2.boundingRect(max_contour))
  max_rect = max_rect[:2] + max_rect[2:] / 2
  weight = likelihood(max_rect[0], max_rect[1], func, image)
  particles = np.ndarray((500, 3), dtype=np.float32)
  particles[:] = [max_rect[0], max_rect[1], weight]
  return particles

##パーティクルフィルタの実装 (アルゴリズム)
ここからが、本当の意味でのパーティクルフィルタの動きの部分です。
パーティクルフィルタは以下4つの手続きを経ます。

1. パーティクルのリサンプリング
2. 予測
3. 尤度(重み)判定
4. 測定

###パーティクルのリサンプリング

リサンプリングでは、乱数を使いつつ、成績の悪いパーティクルを淘汰し、成績の良いパーティクルで置き換えていきます。
weights配列を作るのに使っているcumsum()というメソッドは、累積和を算出してくれるものです。
また、(weights > weight).argmax()とすることで、weights配列をスキャンして、初めてweightよりも大きな値が出てきた時の配列インデックスを返してくれます。

def resample(particles): 
  tmp_particles = particles.copy()
  weights = particles[:, 2].cumsum()
  last_weight = weights[weights.shape[0] - 1]
  for i in xrange(particles.shape[0]):
    weight = np.random.rand() * last_weight
    particles[i] = tmp_particles[(weights > weight).argmax()]
    particles[i][2] = 1.0

###予測

次フレームに向けて、パーティクルを実際に動かします。
varianceで指定した係数にnumpy.random.randn()の結果を掛けたものを足しこみます。
このvariance係数は、ターゲットの動きの激しさに合わせて設定すべき数値です。
パーティクルをランダムに動かすことを「予測」としています。たまたま良い方向に動いたパーティクルは生き残り、悪い方向に向かったパーティクルは淘汰(上書き)される運命です。

def predict(particles, variance=13.0): 
  particles[:, 0] += np.random.randn((particles.shape[0])) * variance
  particles[:, 1] += np.random.randn((particles.shape[0])) * variance

###尤度(重み)判定

パーティクルごとの尤度(重み)を判定します。先ほどの予測の結果が判定されるネタになります。この尤度(重み)は、先に作成したlikelihood()関数を呼び出して算出します。

def weight(particles, func, image): 
  for i in xrange(particles.shape[0]): 
    particles[i][2] = likelihood(particles[i][0], particles[i][1], func, image)
  sum_weight = particles[:, 2].sum()
  particles[:, 2] *= (particles.shape[0] / sum_weight)

###測定

パーティクルを測定して、成績の良いパーティクルが集中している付近の位置を割り出します。

def measure(particles): 
  x = (particles[:, 0] * particles[:, 2]).sum()
  y = (particles[:, 1] * particles[:, 2]).sum()
  weight = particles[:, 2].sum()
  return x / weight, y / weight

###仕上げ

これまで実装した処理を、最後にユーティリティ関数っぽく纏めます。
引数で指定されるmax_frameの間、全く緑成分が見当たらなければパーティクルを再初期化するようにしてあります。

particle_filter_cur_frame = 0

def particle_filter(particles, func, image, max_frame=10):
  global particle_filter_cur_frame
  if image[func(image)].size <= 0:
    if particle_filter_cur_frame >= max_frame:
      return None, -1, -1
    particle_filter_cur_frame = min(particle_filter_cur_frame + 1, max_frame)
  else:
    particle_filter_cur_frame = 0
    if particles is None:
      particles = init_particles(func, image)

  if particles is None:
    return None, -1, -1

  resample(particles)
  predict(particles)
  weight(particles, func, image)
  x, y = measure(particles)
  return particles, x, y

##では、使ってみよう！

ということで、これまでに実装したパーティクルフィルタを利用して、緑色の物体を追跡するプログラムを作ってみましょう。
ここでは、cv2.VideoCapture()で獲得したフレームデータ(BGRイメージ)をHSVに変換し、S(Saturation), V(Value)にそれぞれ、OTSUのスレショルドを掛け、色の濃さ、明るさ共に十分なピクセルのみを生かすという処理を実行しています(H成分をS, Vの合格ピクセルでマスクして0で塗りつぶす)。
緑色(50～85)の範囲を探したいので、0クリアは妥当です。(翻って、赤を検出する時などは0以外の値で塗りつぶさないとおかしなことになります。)

particle_filter()関数に実際に渡しているのは上記フィルタ処理が施されたH成分です。あとはparticle_filter()関数がパーティクルを管理して、緑色部分をうまく追跡してくれると思います。

import cv2
import numpy as np

if __name__ == "__main__": 
  def is_green(region): 
    return (region >= 50) | (region < 85)

  cap = cv2.VideoCapture(0)
  particles = None

  while cv2.waitKey(30) < 0:
    _, frame = cap.read()
	frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV_FULL)
	frame_h = frame_hsv[:, :, 0]
	_, frame_s = cv2.threshold(frame_hsv[:, :, 1], 0, 255, cv2.THRESH_BINARY|cv2.THRESH_OTSU)
	_, frame_v = cv2.threshold(frame_hsv[:, :, 2], 0, 255, cv2.THRESH_BINARY|cv2.THRESH_OTSU)
    frame_h[(frame_s == 0) | (frame_v == 0)] = 0
	
    particles, x, y = particle_filter(particles, is_green, frame_h)
	
    if particles is not None:
      valid_particles = particles[(particles[:, 0] >= 0) & (particles[:, 0] < frame.shape[1]) &
                                  (particles[:, 1] >= 0) & (particles[:, 1] < frame.shape[0])]
      for i in xrange(valid_particles.shape[0]): 
        frame[valid_particles[i][1], valid_particles[i][0]] = [255, 0, 0]
	  p = np.array([x, y], dtype=np.int32)
	  cv2.rectangle(frame, tuple(p - 15), tuple(p + 15), (0, 0, 255), thickness=2)
	
	cv2.imshow('green', frame)

  cap.release()
  cv2.destroyAllWindows()

このパーティクルフィルタは、ノイズや動きに非常に強く、相当しつこく対象物を追いかけてくれます。
また、結果得られる検出位置も大変に安定しています。
一旦パーティクルフィルタを実装してしまえば、あとはlikelihood()関数をどのように実装するかがキモになってくるかと思います。パーティクルの生死は、このlikelihood()に掛かっています。

一度、パーティクルたちの生き残りをかけた必死な動きを観察してみてください。
だんだん、likelihood()に冷酷に評価される1つ1つのパーティクルの必死さが面白くなってきます。的確に追従するパーティクルフィルタですが、その影では数知れないパーティクルが生まれては死んでゆく、そんな過酷な悲哀が感じられます。