高度運転支援向け単眼カメラの実装(1.MATLABでパーティクルフィルタを実装し評価してみる)

パーティクルフィルタの詳しい内容は扱いません。OpenCVのチュートリアルなどに詳しく扱われています。
例題として、特定の色のカラーボールの追跡はよく扱われます。

以下の手順をとることが多いでしょう。

入力画像をHSV変換→ 一定の輝度を持っているピクセルから色相を取り出し → 特徴点
パーティクルフィルタをばら撒く
各パーティクル上、または一定の距離以内存在する特徴点の数*1 → 尤度の決定
尤度の高い座標にカラーボールが存在すると判定
4.で得られた座標を考慮しつつ、カラーボールが次の状態でどこに向かうか推定する(リサンプリング)
3.へ

今回やってみた内容もそれほど変わりませんが、少し工夫をしています。

特徴点の定義

グリーン背景などで、カラーボールを動かす場合は一定の輝度のときの色相を取り出して判定し、特徴点とすることが多いようです。この場合はこの方法はとても有用です(室内の光が少々変わっても追いかけられる)
しかし、車の場合は様々な色をしており、とりまく環境光は絶えず変化しています。加えて昼夜の違いによる大きな輝度変化などがあります。
MATLABでさまざまな画像を入力して、何をパラメータとするとよいか調べたところ、

輝度勾配と色相勾配の内積

が今のところ有用ではないかとみています。
具体的には画像から5x5ピクセル画像を走査し

中心を原点として、周囲8箇所との輝度の差分の絶対値、色相の差分の絶対値を取ります。
それぞれの内積を考慮すると道路と車の区別がつきやすそう、という判断をしました。

これについてはもう少し調査が必要です。現在試験運用中ですが、かなり課題が見えてきています。追加のパラメータが必要だと思います。

パーティクルフィルタの実装

演算量が少ないのは良い事です。
車のドラレコ映像を例にあげます。

衝突回避の目的であれば、この場合必要なデータは

この辺りまでではないでしょうか。標識を読む為にはテンプレートマッチングなどが必要になるため今回は削除しました。
このように考えれば、入力映像のほぼ上半分は演算から除外することができます。

入力される映像は1000x1000ピクセルを想定しています。
特徴点を5x5で演算するので、パーティクルフィルタの演算に必要な領域は1000/5 x 100/5 = 200 x 200ピクセルです。

パーティクルフィルタの行列を以下のように定めます。

PM(x座標, y座標, 重み)

これが、パーティクル数Pだけ増えます。今回のアプローチではPは200~500くらいが良いかと思われます。あまり多いと演算に時間がかかりすぎます。少なすぎると追跡ができません。

PM(x, y, w, P)

ここではいきなりMATLABコードを出します。
別言語で実装を考えている方にもある程度判断がつくようにコメントしてます。私も最終的にSystemVerilog + C/C++での実装が必要なので。

パーティクルの初期化

% パーテクルフィルタ初期化
% xは1~(200-20)までのレンジ、yは画像下半分(100 ~ 200-20)までのレンジ
% 中央の周辺にまずパーティクルを集めてばら撒く
function PM = pm_init(PM,P)
    for m=1:1:P
        PM(1,1,m) =  100 + randi([-70 70]);
        PM(1,2,m) =  100 + randi([0 80]);

    end
    PM(1,3,P) = 1;
end

先ほど述べたように、
パーティクルフィルタの動き回れる領域は200x200です。y座標だけはパーティクルフィルタの動き回れる領域を下半分にしています。(なので実質200x100)

尤度の計算

ランダムにおかれたパーティクルを左上端として、20x20ピクセルを走査し、合計値を格納します。行列Xaは特徴点が格納された200x200の行列です。

% 尤度計算
function PM = likelihood(PM,P,Xa)
    for m = 1:1:P
        x = PM(1,1,m);
        y = PM(1,2,m);
        A = sum(Xa(y:y+19, x:x+19),'all');
        PM(1,3,m) = A; 
    end
end

リサンプリング

今回は単純ランダムサンプリングという手法を用います。パーティクルフィルタの重みの累積和を求めて格納し、重みに応じて乱数を生成し、次のサンプリング座標を決定します。重みが小さい＝成績の悪いフィルタは、良いものに置き換えられます。

% リサンプリング
function [last_w, new_PM] = resample(PM,P)
    new_PM = zeros(1,4,P);          %新しい配列の作成
    weights = cumsum(PM(1,3,:));    %重みの累積和
    last_w = weights(length(weights));
    if last_w == 0
        last_w = 1;
    end

    for n = 1:1:P 
        w = randi(last_w);
        for lp = 1:1:length(weights)
            if w < weights(lp)
                break;
            end
        end
        %disp(lp);
        % lp => index
        new_PM(1,3,n) = PM(1,3,lp); % ウェイトをコピー
        new_PM(1,1,n) = PM(1,1,lp); % xをコピー
        new_PM(1,2,n) = PM(1,2,lp); % yをコピー
    end
end

期待値の計算

期待値の座標から推定して黄色い四角を描きます。
元の入力画像に書き戻す関係で、x,y比率を5倍にしています。

function picRGB = pm_draw(PM, P, picRGB)
    weight = sum(PM(1,3,:));
    x = 0;
    y = 0;
    for m=1:1:P
        x = x + (PM(1,1,m) * PM(1,3,m));
        y = y + (PM(1,2,m) * PM(1,3,m));
    end
    cX = (x * 5 / weight);
    cY = (y * 5 / weight);
    picRGB = insertShape(picRGB,'Rectangle',[cX cY 100 100],'LineWidth',5);　%黄色の四角を描画

end

次の状態の予測

トラッキングしたパーティクルフィルタが次にどこへ移動するかを予測します。ここでパーティクルが動ける領域を可変させると、より良い追跡が可能かもしれません。

function PM = pm_predict(PM,P)
    for m=1:1:P
        x = PM(1,1,m) + randi([-10 10]);
        if x < 1  % x座標の稼働領域を指定
            x = 1;
        elseif x > (200-20)
            x = 200 - 20;
        end
            
        y = PM(1,2,m) + randi([-10 10]);
        if y < 100 % y座標の稼働領域を指定
            y = 100;
        elseif y > (200-20)
            y = 200 - 20;
        end
        PM(1,1,m) = x;
        PM(1,2,m) = y;
        
    end
end

作ってみた

重みの合計値などから、特徴点が異様に少ない時を判別できると思います。この時は全体的に暗めな場合が多いです。
もちろん特徴点が本当にない時と区別をつける必要があります。この辺りは工夫して実装すると面白いと思います。
youtu.be

参考

jp.mathworks.com
動画像理解技術基礎講座(中部大学) http://mprg.jp/data/VU/pdf/VU4-ObjectTracking.pdf
パーティクル・フィルタをやさしく解説 - Allisone

*1:あくまで例です。なにをもって尤度とするかは実装に依ります

lynxeyedの電音鍵盤

MBDとFPGAと車載で使うデバイスの備忘録