OpenCV學習之Condensation演算法

最近在做一個多目標跟蹤的問題，需要用到openCV中的Camshift演算法，Kalman濾波演算法 和Condensation演算法，前兩個演算法的資料相對較多，在"學習OpenCV"這本書中講解的也比較明白，但是Condensation演算法的資料 相對較少，沒有一個詳細的介紹，最後只在日本的論壇上找到一段使用Condensation的範例程式碼，總算看明白了，建議需要學習的朋友們先去搞懂 Kalman濾波演算法，再來看這算代碼就容易了。將注釋翻譯之後的代碼如下所示（需要注意的是Condensation的聲明是包含在cvAux.h這個 標頭檔裡面，原來應該是在cv.h裡面，但是編譯一直沒有通過，後來仔細看菜發現這個貌似被挪了位置）：

// Condensation_demo.cpp : 定義控制台應用程式的進入點。

#include "stdafx.h"
#ifdef _CH_
#pragma package <opencv>
#endif
#include "stdafx.h"
#ifndef _EiC
#include "cv.h"
#include "cvAux.h"
#include "highgui.h"
#include "cxcore.h"
#include <stdio.h>
#include <ctype.h>
#endif

// 從圖片的x、y座標處返回相應的色調、飽和度和亮度

int getpixel(IplImage *image, int x, int y, int *h, int *s, int *v){
    *h =(uchar) image->imageData[y *image->widthStep+x * image->nChannels];
    *s =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 1];
    *v =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 2];
    return 0;
}
//--------------------------------------------------------------------------------

 

int main( int argc, char** argv ){
    CvCapture* capture = 0;
    IplImage* image = 0;
    IplImage* HSV = 0;

    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0]))){
        capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
    }
    else if( argc == 2 ){
        capture = cvCaptureFromAVI( argv[1] );
    }

    if( !capture ){
        fprintf(stderr,"Could not initialize capturing.../n");
        return -1;
    }

    printf( "Hot keys: /n"
        "/tESC - quit the program/n");

    //建立Normal視窗

    cvNamedWindow("Normal", CV_WINDOW_AUTOSIZE );

    //Condensation結構體初始化-------------------------------------------------

    int DP=2; // 狀態向量的維數

    int MP=2; // 觀測向量的維數

    int SamplesNum=300; // 樣本粒子的數量

    CvConDensation* ConDens=cvCreateConDensation( DP, MP, SamplesNum );
    //-----------------------------------------------------------------------

    //Condensation結構體中一些參數的初始化-----------------------------------

    CvMat* lowerBound; // 下界

    CvMat* upperBound; // 上界

    lowerBound = cvCreateMat(2, 1, CV_32F);
    upperBound = cvCreateMat(2, 1, CV_32F);
    //設定粒子座標的上下界為視窗大小640*480

    cvmSet( lowerBound, 0, 0, 0.0 ); cvmSet( upperBound, 0, 0, 640.0 );
    cvmSet( lowerBound, 1, 0, 0.0 ); cvmSet( upperBound, 1, 0, 480.0 );

    cvConDensInitSampleSet(ConDens, lowerBound, upperBound);
    //-----------------------------------------------------------------------

    //設定視窗的中心為追蹤的初始點------------------------------

    for(int i=0; i < SamplesNum; i++){
        ConDens->flSamples[i][0]+=320.0;
        ConDens->flSamples[i][1]+=240.0;
    }
    //-----------------------------------------------------------------------

    //遷移矩陣的初始化----------------------------

    ConDens->DynamMatr[0]=1.0;ConDens->DynamMatr[1]=0.0;
    ConDens->DynamMatr[2]=0.0;ConDens->DynamMatr[3]=1.0;
    //-----------------------------------------------------------------------

    for(;;){
        IplImage* frame = 0;
        int c;
        int X,Y,XX,YY;
        int H,S,V;

        frame = cvQueryFrame( capture );
        if( !frame ){
            break;
        }

        if( !image ){
            image = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
            HSV = cvCreateImage( cvGetSize(frame), 8, 3 );
            HSV->origin = frame->origin;
        }

        cvCopy( frame, image, 0 );
        cvCvtColor(image ,HSV , CV_BGR2HSV);

        //粒子的信賴度計算，信賴度需要自己建模---------------------------------------------------

        for(int i=0; i < SamplesNum; i++){
            X=(int)ConDens->flSamples[i][0];
            Y=(int)ConDens->flSamples[i][1];

            if(X>=0 && X<=640 && Y>=0 && Y<=480){ //粒子的座標在視窗範圍之內

                getpixel(HSV, X, Y, &H, &S, &V);
                if(H<=19 && S>=48){ // 膚色的判定 //H<=19 S>=48

                    cvCircle(image, cvPoint(X,Y), 4, CV_RGB(255,0,0), 1);
                    ConDens->flConfidence[i]=1.0;
                }
                else{
                    ConDens->flConfidence[i]=0.0;
                }
            }
            else{
                ConDens->flConfidence[i]=0.0;
            }
        }
        //--------------------------------------------------------------------------

        //更新濾波器狀態

        cvConDensUpdateByTime(ConDens);

        cvShowImage( "Normal", image );
        c = cvWaitKey(20);

        if( c == 27 ){
            break;
        }
    }

    //釋放記憶體------------------------------------

    cvReleaseImage(&image);
    cvReleaseImage(&HSV);
    cvReleaseConDensation(&ConDens);
    cvReleaseMat( &lowerBound );
    cvReleaseMat( &upperBound );
    cvReleaseCapture( &capture );
    cvDestroyWindow("Normal");
    //---------------------------------------------

    return 0;
}

#ifdef _EiC
main(1,"condensation.cpp");
#endif