【Python/OpenCV】パーティクルフィルタで物体追跡

Python版OpenCVでパーティクルフィルタ（粒子フィルタ）を実装し、物体追跡する方法をソースコード付きで解説します。

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

パーティクルフィルタ(Particle filter)とは、確率分布に基づく時系列データの予測手法です。粒子フィルターや逐次モンテカルロ法とも呼ばれます。
多数のパーティクル（粒子）を用いて、現在の状態から将来の状態を推定します。画像処理の分野では、物体の追跡などに応用されています。

以下の画像は、パーティクルフィルタで物体追跡するときの簡単なイメージです。

パーティクルフィルタでは、正確な位置を求めるのでなく、「過去の状態から、追跡対象が次にこの辺に来る確率が高いだろう」という推定をします。
パーティクルフィルタは計算量が少ないため、リアルタイムな物体追跡に応用されています。

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

パーティクルフィルタによる物体追跡の流れは以下のとおりです。

1. リサンプリング
   • 前フレームでの尤度（重み）に従って、パーティクルを撒き直します。(追跡対象の周りにパーティクルをばら撒く)
   • ※初期状態の場合、前フレームがないので追跡対象の周辺に一様にパーティクルを撒きます。
   • 尤度とは、「追跡したい対象物らしさ」のことです。例えば、「赤色の物体追跡」を行う場合、各パーティクルの周辺領域にある赤色の存在率などを尤度とします。
2. 推定
   • 適当なモデルを使って現フレームにおける追跡対象の位置を推定し、パーティクルを少し動かします。
   • 動画のような2次元座標の場合、等速直線運動や適当な乱数のモデルが使われます
3. 観測
   • 現フレームにおける各パーティクルの尤度と重み（正規化）を計算します。つまり、「②推定」の答え合わせをして実際の追跡対象の位置に近いパーティクルの重みを大きくします。
   • 重みが大きいパーティクルが集中している領域が追跡対象となります。

【画像処理】パーティクルフィルタによる物体追跡の原理

この記事では、画像処理におけるパーティクルフィルタによる動体追跡の原理について解説します。

algorithm.joho.info

サンプルコード

サンプルプログラムのソースコードです。
このプログラムは、動画内の特定の色を追跡し、その結果をリアルタイムで表示するものです。各ステップでパーティクルフィルタの処理を行い、追跡対象の位置を推定します。

実行例

サンプルプログラムの実行結果です。赤色の物体を追跡しています。

コードの解説

上記サンプルコードのポイントを解説します。

• 以下のis_target関数は、指定された色範囲（Hue値が30以下または150以上）に該当するかを判定します。今回は追跡対象である赤色を追跡するために使用されます。

# 追跡対象の色範囲（Hueの値域）
def is_target(roi):
    return (roi <= 30) | (roi >= 150)

• 以下のmax_moment_point関数は、マスク画像から最大のブロブ（連結成分）の中心座標を計算します。

# マスクから面積最大ブロブの中心座標を算出
def max_moment_point(mask):
    # 画像のラベリングを行い、各ブロブの統計情報を取得します。
    label = cv2.connectedComponentsWithStats(mask)
    data = np.delete(label[2], 0, 0)   # ブロブのデータ
    center = np.delete(label[3], 0, 0)  # 各ブロブの中心座標
    moment = data[:, 4]                 # 各ブロブの面積
    max_index = np.argmax(moment)      # 面積最大のインデックス
    return center[max_index]           # 面積最大のブロブの中心座標を返す

• 以下のinitialize関数は、画像内の赤色領域(追跡対象)をマスクし、最大ブロブの中心座標を初期パーティクルとして設定します。（パーティクルの初期化）

# パーティクルの初期化
def initialize(img, N):
    mask = img.copy()                  # 画像のコピー
    mask[is_target(mask) == False] = 0  # マスク画像の作成（追跡対象外の色なら画素値0）
    x, y = max_moment_point(mask)      # マスクから面積最大ブロブの中心座標を算出
    w = calc_likelihood(x, y, img)     # 尤度の算出
    ps = np.ndarray((N, 3), dtype=np.float32)  # パーティクル格納用の配列を生成
    ps[:] = [x, y, w]                  # パーティクル用配列に中心座標と尤度をセット
    return ps

• 以下のresampling関数は、パーティクルの重みに基づいて新しいパーティクルを生成します。(リサンプリング)

# 1.リサンプリング(前状態の重みに応じてパーティクルを再選定)
def resampling(ps):
    # パーティクルの重みの累積和を計算
    ws = ps[:, 2].cumsum()
    last_w = ws[ws.shape[0] - 1]
    # 新しいパーティクル用の空配列を生成
    new_ps = np.empty(ps.shape)
    # 前状態の重みに応じてパーティクルをリサンプリング（重みは1.0）
    for i in range(ps.shape[0]):
        # ランダムな重みを生成
        w = np.random.rand() * last_w
        # 累積重みがランダムな重みを超える最初のパーティクルを選択
        new_ps[i] = ps[(ws > w).argmax()]
        # 新しいパーティクルの重みを1.0に設定
        new_ps[i, 2] = 1.0

    return new_ps

• 以下のpredict_position関数は、パーティクルの位置をランダムに少しずらします。(パーティクルの位置予測)

# 2.推定（パーティクルの位置）
def predict_position(ps, var=13.0):
    # 正規分布と与えられた分散に従ってランダムに少し位置をずらす
    ps[:, 0] += np.random.randn((ps.shape[0])) * var
    ps[:, 1] += np.random.randn((ps.shape[0])) * var

• 以下の*calc_likelihood関数は、指定された座標を中心とする領域内の追跡対象の色の割合を計算します。(尤度の計算)

# 尤度の算出
def calc_likelihood(x, y, img, w=30, h=30):
    # 画像から座標(x,y)を中心とする幅w, 高さhの矩形領域の全画素を取得
    x1, y1 = max(0, x-w/2), max(0, y-h/2)
    x2, y2 = min(img.shape[1], x+w/2), min(img.shape[0], y+h/2)
    x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2)
    roi = img[y1:y2, x1:x2]

    # 領域内の追跡対象（今回だと赤色）の画素数を計算
    count = roi[is_target(roi)].size

    # 矩形領域中に含まれる追跡対象(今回は赤色)の存在率を尤度として計算
    return (float(count) / img.size) if count > 0 else 0.0001

• 以下のcalc_weight関数は、各パーティクルの尤度に基づいて重みを計算し、正規化します。（パーティクルの重み付け）

def calc_weight(ps, img):
    # 尤度に従ってパーティクルの重み付け
    for i in range(ps.shape[0]):
        ps[i][2] = calc_likelihood(ps[i, 0], ps[i, 1], img)

    # 重みの正規化
    ps[:, 2] *= ps.shape[0] / ps[:, 2].sum()

• 以下のobserver関数は、重み付き平均を計算してパーティクルの推定位置を返します。（観測）

# 3.観測（全パーティクルの重み付き平均を取得）
def observer(ps, img):
    # パーティクルの重み付け
    calc_weight(ps, img)
    # 重み付きのx座標の合計を計算
    x = (ps[:, 0] * ps[:, 2]).sum()

    # 重み付きのy座標の合計を計算
    y = (ps[:, 1] * ps[:, 2]).sum()
    # 重み付き平均を計算して返す
    return (x, y) / ps[:, 2].sum()

• 以下のparticle_filter関数は、パーティクルフィルタの処理をまとめた関数です。原理通りに並んでいることがわかるかと思います。

def particle_filter(ps, img, N=300):
    # パーティクルが無い場合
    if ps is None:
        ps = initialize(img, N)  # パーティクルを初期化

    ps = resampling(ps)    # 1.パーティクルをリサンプリング
    predict_position(ps)   # 2.パーティクルの位置を推定
    x, y = observer(ps, img)  # 3.観測
    return ps, int(x), int(y)

• 以下はメイン関数です。

def main():
    # パーティクル格納用の変数
    ps = None

    # 動画ファイルのキャプチャ
    cap = cv2.VideoCapture("C:/github/sample/python/opencv/dataset/videos/red_marker.mp4")

    while cv2.waitKey(30) < 0:
        ret, frame = cap.read()
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV_FULL)
        h = hsv[:, :, 0]

        # S, Vを2値化（大津の手法）
        ret, s = cv2.threshold(hsv[:, :, 1], 0, 255,
                               cv2.THRESH_BINARY | cv2.THRESH_OTSU)
        ret, v = cv2.threshold(hsv[:, :, 2], 0, 255,
                               cv2.THRESH_BINARY | cv2.THRESH_OTSU)
        h[(s == 0) | (v == 0)] = 100

        # パーティクルフィルタで位置を推定
        ps, x, y = particle_filter(ps, h, 300)

        if ps is None:
            continue

        # 画像の範囲内にあるパーティクルのみ取り出し
        ps1 = ps[(ps[:, 0] >= 0) & (ps[:, 0] < frame.shape[1]) &
                 (ps[:, 1] >= 0) & (ps[:, 1] < frame.shape[0])]

        # パーティクルを赤色で塗りつぶす
        for i in range(ps1.shape[0]):
            frame[int(ps1[i, 1]), int(ps1[i, 0])] = [0, 0, 200]

        # パーティクルの集中部分を赤い矩形で囲む
        cv2.rectangle(frame, (x-20, y-20), (x+20, y+20), (0, 0, 200), 5)

        cv2.imshow('Result', frame)

    cap.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()

関連ページ

【PythonとOpenCVで画像処理超入門】使い方とサンプルコードを解説

Python版OpenCVで画像処理プログラミングを行う方法を入門者向けにソースコード付きで解説するページです。

python.joho.info

2024.06.30