Last modified 7 years ago Last modified on 07/23/09 14:59:24

物体の追跡

MISTを用いた実践的なプログラム例として、ここでは動画像中の物体の追跡を行います。
以下に、パーティクルフィルタというアルゴリズムを用いた物体追跡を紹介します。

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

物体の検出追跡を同時に行う、逐次追跡アルゴリズムです。
逐次追跡(追跡の終点を予め必要としない追跡)であり、かつ、計算も軽いので、リアルタイム処理が可能なアルゴリズムです。
ここではおおまかな処理しか説明しませんので、パーティクルフィルタについて詳しく知りたい方は、以下の文献を参考にしてください。
【参考文献:Michael Isard, Andrew Blake: Condensation-conditional density propagation for visual tracking,International Journal of Computer Vision,Vol.29,No.1,pp.5-28,1998】

おおまかな処理の流れ

パーティクルフィルタは、現状態から起こりうる多数の次状態を、多数(数百or数千)のパーティクル(粒子)に見立て、全パーティクルの尤度に基づいた重みつき平均を次状態として予測しながら追跡を行っていくアルゴリズムです。
以下は、おおまかな処理の流れになります。

  1. リサンプリング:前状態における尤度(重み)に基づいて、パーティクルを選び直す。
  2. 予測:各パーティクルについて、前状態から現状態の予測を行う。
  3. 重み付け:現状態における各パーティクルの尤度を求める。
  4. 観測:結果を出力するために(今回は、全パーティクルの重みつき平均を現状態として)観測する。

これら4つの処理は、MISTを用いて以下のように書けます。
ここでは、最も単純な例として、検出・追跡したい物体の画像中の位置(x座標, y座標)のみを状態要素に持つ場合を考えます。

パーティクルを構造体として準備

struct particle_type
{
    int     x;
    int     y;
    double  weight;
    particle_type( const int x0 = 0, const int y0 = 0, const double weight0 = 0.0 )
         : x( x0 ), y( y0 ), weight( weight0 )
    {
    }
};

1.リサンプリング

前状態における尤度(重み)に基づいて、パーティクルを選び直す。

サンプルコード1

bool resample           ( mist::array1< particle_type > &particles, 
                          mist::uniform::random         &urnd )
{
    // 累積重みの計算
    mist::array1< double > weights( particles.size( ) );
    weights( 0 ) = particles( 0 ).weight;
    for( size_t i = 1 ; i < weights.size( ) ; i ++ )
    {
        weights( i ) = weights( i - 1 ) + particles( i ).weight;
    }

    // 重みを基準にパーティクルをリサンプリングして重みを1.0に
    mist::array1< particle_type > tmp_particles = particles;
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        const double weight = urnd.real2( ) * weights( weights.size( ) - 1 );
        size_t n = 0;
        while( weights( ++ n ) < weight );
        particles( i ) = tmp_particles( n );
        particles( i ).weight = 1.0;
    }

    return true;
}

2.予測

各パーティクルについて、前状態から現状態の予測を行う。

サンプルコード2

bool predict            ( mist::array1< particle_type > &particles,
                          mist::gauss::random           &grnd )
{
    const double variance = 13.0;
    
    // 位置の予測
    // 「次状態もほぼ同じ位置(ほとんど動かない)」と仮定、分散(13.0)は実験的に決定
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        double vx = grnd.generate( ) * variance;
        double vy = grnd.generate( ) * variance;

        particles( i ).x += static_cast< int >( vx );
        particles( i ).y += static_cast< int >( vy );
    }

    return true;
}

3.重み付け

現状態における各パーティクルの尤度を求め、そのパーティクルの重みを決める。

RGBが要素となる2次元画像の設定

typedef mist::array2< mist::rgb< unsigned char > > image_type;

尤度を計算する

サンプルコード3-1

double likelihood       ( const int         &x,
                          const int         &y,
                          const image_type  &image )
{
    const int width  = 30;
    const int height = 30;
    
    // 今回はパーティクルを中心とした30x30の矩形領域に
    // 指定した範囲の色の存在率を尤度とした
    size_t count = 0;
    for( int j = y - height / 2 ; j < y + height / 2 ; j ++ )
    {
        for( int i = x - width / 2 ; i < x + width / 2 ; i ++ )
        {               
            if( is_in_image( i, j, image ) && is_yellow( image( i, j ) ) )
            {
                count ++;
            }
        }
    }
    if( count == 0 )
    {
        return 0.0001;
    }
    else
    {
        return static_cast< double >( count ) / ( width * height );
    }
}

尤度から、そのパーティクルの重みを決定する

サンプルコード3-2

bool weight         ( mist::array1< particle_type > &particles,
                      const image_type              &image )
{
    // 尤度に従いパーティクルの重みを決定する
    double sum_weight = 0.0;
    for( size_t i = 0 ; i < particles.size( ) ; i++ )
    {
        particles( i ).weight = likelihood( particles( i ).x, particles( i ).y, image );
        sum_weight += particles( i ).weight;
    }
    // 重みの正規化
    for( size_t i = 0; i < particles.size( ) ; i++ )
    {
        particles( i ).weight = ( particles( i ).weight / sum_weight ) * particles.size( );
    }

    return true;
}

4.観測

全パーティクルの重みつき平均を現状態として観測する。

サンプルコード4

bool measure            ( const mist::array1< particle_type >   &particles,
                          particle_type                         &result_particle )
{
    double  x = 0.0;
    double  y = 0.0;
    double  weight = 0.0;
    
    // 重み和
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        x       += particles[ i ].x         * particles[ i ].weight;
        y       += particles[ i ].y         * particles[ i ].weight;
        weight  += particles[ i ].weight;
    }
    
    // 正規化
    result_particle.x       = static_cast< int >( x / weight );
    result_particle.y       = static_cast< int >( y / weight );
    result_particle.weight  = 1.0;      // 任意

    return true;
}

サンプルプログラム

パーティクルフィルタを用いて"色特徴を用いて動画中のテニスボールを追跡する"というタスクを行うサンプルプログラムです。
入力動画はMPEG形式で、input.mpgという入力映像が必要です。
MPEGの読み書きについては、ビデオの入出力を参考にしてください。
サンプル動画はこちらから(約3MB)。

動作確認計算機環境

コンパイラ CPU メモリ 実行時間[s] fps
Visual C++ Pentium IV 2.8GHz 1GB 30.57 34.2
GCC3.4.2 Pentium IV 3.0GHz 2GB 31.43 33.3
#include <mist/mist.h>
#include <mist/io/video.h>
#include <mist/random.h>
#include <mist/timer.h>
#include <mist/drawing.h>

#include <ctime>

// RGBが要素となる2次元画像の設定
typedef mist::array2< mist::rgb< unsigned char > > image_type;  

// パーティクルフィルタ変数の構造体
struct particle_type
{
    int     x;
    int     y;
    double  weight;

    particle_type( const int x0 = 0, const int y0 = 0, const double weight0 = 0.0 ) : x( x0 ), y( y0 ), weight( weight0 )
    {
    }
};

bool    initialize      ( mist::array1< particle_type > &particles, const image_type &image );
bool    resample        ( mist::array1< particle_type > &particles, mist::uniform::random &urnd );
bool    predict         ( mist::array1< particle_type > &particles, mist::gauss::random &grnd );
bool    weight          ( mist::array1< particle_type > &particles, const image_type &image );
bool    measure         ( const mist::array1< particle_type > &particles, particle_type &result_particle );
bool    show_rectangle  ( const particle_type &result_particle, image_type &image, mist::video::encoder &out_video );
bool    show_particle   ( const mist::array1< particle_type > &particles, image_type &image, mist::video::encoder &out_video );
double  likelihood      ( int x, int y, const image_type &image );
bool    is_in_image     ( int x, int y, const image_type &image );
bool    is_yellow       ( const mist::rgb< unsigned char > &color );

int main( int argc, char *argv[] )
{
    image_type                      image;
    particle_type                   result_particle;
    mist::array1< particle_type >   particles( 500 );   // パーティクル数500とする
    mist::video::decoder            in_video;
    mist::video::mpeg1::encoder     out_rect;
    mist::video::mpeg1::encoder     out_particle;
    mist::uniform::random           urnd( std::clock( ) );
    mist::gauss::random             grnd( std::clock( ) );
    mist::timer                     time;

    // 入力mpegファイルのオープン
    in_video.open( "input.mpg" );
    in_video.dump( );

    // 出力mpegファイル(結果表示用)のオープン
    out_rect.open( "out_rect.mpg" );
    out_rect.dump( );

    // 出力mpegファイル(パーティクル表示用)のオープン
    out_particle.open( "out_particle.mpg" );
    out_particle.dump( );

    // パーティクルの初期値を決定する
    unsigned int i = 0;
    std::cout << "\r" << "フレーム:" << i;
    in_video >> image;
    initialize( particles, image ); 

    while( !in_video.is_eof( ) )
    {
        in_video >> image;

        std::cout << "\r" << "フレーム:" << i ++;

        // パーティクルのリサンプリング
        resample( particles, urnd );

        // 各パーティクルの予測
        predict( particles, grnd );

        // 各パーティクルを尤度に従い重み付け
        weight( particles, image );

        // 観測
        measure( particles, result_particle );

        // 出力
        show_rectangle( result_particle, image, out_rect );
        show_particle( particles, image, out_particle );
    }

    // 入力・出力mpegファイルのクローズ
    in_video.close( );
    out_rect.close( );
    out_particle.close( );

    // 経過時間を表示
    std::cout << std::endl << time << std::endl;

    return( 0 );
}

// パーティクルの初期値を決定する
bool initialize( mist::array1< particle_type > &particles, const image_type &image )
{
    particle_type max_particle( 0, 0, 0.0 );

    // 最も尤度の高いピクセルを探索
    for( size_t j = 0 ; j < image.height( ) ; j ++ )
    {
        for( size_t i = 0 ; i < image.width( ) ; i ++ )
        {
            double weight = likelihood( i, j, image );
            if( weight > max_particle.weight )
            {
                max_particle = particle_type( i, j, weight );
            }
        }
    }

    // すべてのパーティクルの値を最尤値で設定する
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        particles[ i ] = max_particle;
    }

    return( true );
}

// パーティクルのリサンプリング
bool resample( mist::array1< particle_type > &particles, mist::uniform::random &urnd )
{
    // 累積重みの計算
    mist::array1< double > weights( particles.size( ) );
    weights( 0 ) = particles( 0 ).weight;
    for( size_t i = 1 ; i < weights.size( ) ; i ++ )
    {
        weights( i ) = weights( i - 1 ) + particles( i ).weight;
    }

    // 重みを基準にパーティクルをリサンプリングして重みを1.0に
    mist::array1< particle_type > tmp_particles = particles;
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        const double weight = urnd.real2( ) * weights( weights.size( ) - 1 );
        size_t n = 0;
        while( weights( ++ n ) < weight );
        particles( i ) = tmp_particles( n );
        particles( i ).weight = 1.0;
    }

    return( true );
}


// 各パーティクルの予測
bool predict( mist::array1< particle_type > &particles, mist::gauss::random &grnd )
{
    const double variance = 13.0;

    // 位置の予測
    // 「次状態もほぼ同じ位置(ほとんど動かない)」と仮定、分散(13.0)は実験的に決定
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        double vx = grnd.generate( ) * variance;
        double vy = grnd.generate( ) * variance;

        particles( i ).x += static_cast< int >( vx );
        particles( i ).y += static_cast< int >( vy );
    }

    return( true );
}

// 各パーティクルを尤度に従い重み付け
bool weight( mist::array1< particle_type > &particles, const image_type &image )
{
    // 尤度に従いパーティクルの重みを決定する
    double sum_weight = 0.0;
    for( size_t i = 0 ; i < particles.size( ) ; i++ )
    {
        particles( i ).weight = likelihood( particles( i ).x, particles( i ).y, image );
        sum_weight += particles( i ).weight;
    }
    // 重みの正規化
    for( size_t i = 0; i < particles.size( ) ; i++ )
    {
        particles( i ).weight = ( particles( i ).weight / sum_weight ) * particles.size( );
    }

    return( true );
}

// 観測
bool measure( const mist::array1< particle_type > &particles, particle_type &result_particle )
{
    double  x = 0.0;
    double  y = 0.0;
    double  weight = 0.0;

    // 重み和
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        x       += particles[ i ].x         * particles[ i ].weight;
        y       += particles[ i ].y         * particles[ i ].weight;
        weight  += particles[ i ].weight;
    }

    // 正規化
    result_particle.x       = static_cast< int >( x / weight );
    result_particle.y       = static_cast< int >( y / weight );
    result_particle.weight  = 1.0;      // 任意

    return( true );
}

// 尤度を計算する
double likelihood( int x, int y, const image_type &image )
{
    const int width  = 30;
    const int height = 30;

    // 今回はパーティクルを中心とした30x30の矩形領域に
    // 指定した範囲の色の存在率を尤度とした
    size_t count = 0;
    for( int j = y - height / 2 ; j < y + height / 2 ; j ++ )
    {
        for( int i = x - width / 2 ; i < x + width / 2 ; i ++ )
        {               
            if( is_in_image( i, j, image ) && is_yellow( image( i, j ) ) )
            {
                count++;
            }
        }
    }
    if( count == 0 )
    {
        return( 0.0001 );
    }
    else
    {
        return( static_cast< double >( count ) / ( width * height ) );
    }
}


// 以下は、パーティクルフィルタには直接関係のない関数

// (x,y)が画像内に収まっているかどうか
bool is_in_image( int x, int y, const image_type &image )
{
    return( 0 <= x && x < static_cast< int >( image.width( ) ) && 0 <= y && y < static_cast< int >( image.height( ) ) );
}

// color(RGB値)が指定した範囲の色か
bool is_yellow( const mist::rgb< unsigned char > &color )
{
    return( color.r > 200 && color.g > 200 && color.b < 150 );
}

// 結果を矩形で出力
bool show_rectangle( const particle_type &particle, image_type &image, mist::video::encoder &out_video )
{
    const size_t width  = 30;
    const size_t height = 30;

    const int rect_top      = particle.y - static_cast< int >( height / 2 );
    const int rect_bottom   = particle.y + static_cast< int >( height / 2 );
    const int rect_left     = particle.x - static_cast< int >( width  / 2 );
    const int rect_right    = particle.x + static_cast< int >( width  / 2 );

    //// 検出・追跡の結果を赤色の矩形で表示
    mist::draw_line( image, rect_left,  rect_top,       rect_right, rect_top,       mist::rgb< unsigned char >( 255, 0, 0 ) );  //上端
    mist::draw_line( image, rect_left,  rect_bottom,    rect_right, rect_bottom,    mist::rgb< unsigned char >( 255, 0, 0 ) );  //下端
    mist::draw_line( image, rect_left,  rect_top,       rect_left,  rect_bottom,    mist::rgb< unsigned char >( 255, 0, 0 ) );  //左端
    mist::draw_line( image, rect_right, rect_top,       rect_right, rect_bottom,    mist::rgb< unsigned char >( 255, 0, 0 ) );  //右端

    out_video << image;

    return( true );
}

// パーティクルを出力
bool show_particle( const mist::array1< particle_type > &particles, image_type &image, mist::video::encoder &out_video )
{   
    // パーティクルのあるピクセルは、画素値=rgb(255,0,0)
    for( size_t i = 0 ; i < particles.size( ) ; i ++ )
    {
        if( is_in_image( particles( i ).x, particles( i ).y, image ) )
        {
            image( particles( i ).x, particles( i ).y ) = mist::rgb< unsigned char >( 255, 0 ,0 );
        }
    }

    out_video << image;

    return( true );
}

実行結果

オリジナル画像

パーティクル付加画像

矩形出力画像

Attachments