Example of how to use the OpenCV Particle Filter (opencv Ver 2.4.9)

This is example of OpenCV particle filter

#include < iostream>
#include < vector>

#include < opencv2/opencv.hpp>
#include < opencv2/legacy/legacy.hpp>

#ifdef _DEBUG           
#pragma comment(lib, "opencv_core249d.lib")   
#pragma comment(lib, "opencv_imgproc249d.lib")   //MAT processing   
#pragma comment(lib, "opencv_objdetect249d.lib") //HOGDescriptor   
//#pragma comment(lib, "opencv_gpu249d.lib")   
//#pragma comment(lib, "opencv_features2d249d.lib")   
#pragma comment(lib, "opencv_highgui249d.lib")   
#pragma comment(lib, "opencv_ml249d.lib")   
//#pragma comment(lib, "opencv_stitching249d.lib");   
//#pragma comment(lib, "opencv_nonfree249d.lib");   
#pragma comment(lib, "opencv_video249d.lib")
#pragma comment(lib, "opencv_legacy249d.lib")
#else   
#pragma comment(lib, "opencv_core249.lib")   
#pragma comment(lib, "opencv_imgproc249.lib")   
#pragma comment(lib, "opencv_objdetect249.lib")   
//#pragma comment(lib, "opencv_gpu249.lib")   
//#pragma comment(lib, "opencv_features2d249.lib")   
#pragma comment(lib, "opencv_highgui249.lib")   
#pragma comment(lib, "opencv_ml249.lib")   
//#pragma comment(lib, "opencv_stitching249.lib");   
//#pragma comment(lib, "opencv_nonfree249.lib");   
#pragma comment(lib, "opencv_video249d.lib")   
#endif   




using namespace std;


#define drawCross( center, color, d ) line( img, cv::Point( center.x - d, center.y - d ), cv::Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); line( img, cv::Point( center.x + d, center.y - d ), cv::Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )


struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

vector< cv::Point> mouseV, particleV;
int counter = -1;

// Define this to proceed one click at a time.
//#define CLICK 1
#define PLOT_PARTICLES 1

void on_mouse(int event, int x, int y, int flags, void* param) {
#ifdef CLICK
 if (event == CV_EVENT_LBUTTONUP) 
#endif
 {
  last_mouse = mouse_info;
  mouse_info.x = x;
  mouse_info.y = y;
  counter = 0;
 }
}

void printMat(CvMat* mat)
{
    
    for(int i=0; i< mat->rows; i++)
    {
        if(i==0)
        {
            for(int j=0; j< mat->cols; j++)  printf("%10d",j+1);
        }

        printf("\n%4d: ",i+1);
        for(int j=0; j< mat->cols; j++)
        {

            printf("%10.2f",cvGet2D(mat,i,j).val[0]);
        }
    }
    
}


int main () {
 cv::Mat img(650, 650, CV_8UC3);
 char code = (char)-1;

 cv::namedWindow("mouse particle");
 cv::setMouseCallback("mouse particle", on_mouse, 0);

 cv::Mat_< float> measurement(2,1); 
 measurement.setTo(cv::Scalar(0));

 int DP = 2;
 int MP = 2;
 int nParticles = 250;
 float xRange = 650.0;
 float yRange = 650.0;

 float minRange[] = { 0, 0, -xRange, -yRange };
 float maxRange[] = { xRange, yRange, xRange, yRange };
 CvMat LB, UB;
 cvInitMatHeader(&LB, DP, 1, CV_32FC1, minRange); 
 cvInitMatHeader(&UB, DP, 1, CV_32FC1, maxRange);

 CvConDensation* condens = cvCreateConDensation(DP, MP, nParticles);

 
 cvConDensInitSampleSet(condens, &LB, &UB); //Lower Bound, Upper Bound

 // The OpenCV documentation doesn't tell you to initialize this
 // matrix, but you have to do it.  For this 2D example, we're just
 // using a 2x2 identity matrix.  I'm sure there's a slicker way to
 // do this, left as an exercise for the reader.
 condens->DynamMatr[0] = 1.0;
 condens->DynamMatr[1] = 0.0;
 condens->DynamMatr[2] = 0.0;
 condens->DynamMatr[3] = 1.0;

 for (int i = 0; i < condens->SamplesNum; i++) {
  cv::Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
  drawCross(partPt , cv::Scalar(255,0,(int)(i * 255.0/(float)condens->SamplesNum)), 2);
 }



 for(;;) {

  if (mouse_info.x < 0 || mouse_info.y < 0) {
   imshow("mouse particle", img);
   cv::waitKey(30);
   continue;
  }

  mouseV.clear();
  particleV.clear();

  for(;;) {
   code = (char)cv::waitKey(100);

   if( code > 0 )
    break;

#ifdef CLICK
   if (counter++ > 0) {
    continue;
   } 
#endif

   measurement(0) = mouse_info.x;
   measurement(1) = mouse_info.y;

   cv::Point measPt(measurement(0),measurement(1));
   mouseV.push_back(measPt);

   // Clear screen
   img = cv::Scalar::all(100);

   for (int i = 0; i < condens->SamplesNum; i++) {

    float diffX =  (measurement(0) - condens->flSamples[i][0])/xRange;
    float diffY =  (measurement(1) - condens->flSamples[i][1])/yRange;

    condens->flConfidence[i] = exp(-100.0f * ((diffX * diffX + diffY * diffY)));

    // plot particles
#ifdef PLOT_PARTICLES
    cv::Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
    drawCross(partPt , 
     cv::Scalar(255,0,(int)(i * 255.0/(float)condens->SamplesNum)), 
     2);
#endif

   }

   cvConDensUpdateByTime(condens);

   cv::Point statePt(condens->State[0], condens->State[1]);
   particleV.push_back(statePt);

   for (int i = 0; i < mouseV.size() - 1; i++) {
    line(img, mouseV[i], mouseV[i+1], cv::Scalar(255,255,0), 1);
   }
   for (int i = 0; i < particleV.size() - 1; i++) {
    line(img, particleV[i], particleV[i+1], cv::Scalar(0,255,0), 1);
   }

   // plot points
   drawCross( statePt, cv::Scalar(255,0,0), 3 );
   drawCross( measPt, cv::Scalar(0,0,255), 3 );

   imshow( "mouse particle", img );
  }

  if( code == 27 || code == 'q' || code == 'Q' )
   break;
 }

 return 0;
}

Opencv Condensation(particle filter) example source code

Dear Jihad anwar

Here is source code you asking.
I don't remember equation about particle filter well now.
But source code is still run well. ^^
I hope to help your study.

Note. Opencv 2.4.9 and 32 bit.

Thank you.

#include < iostream>
#include < vector>

#include <  stdio.h>      
#include <  opencv2\opencv.hpp>    
#include < opencv2\legacy\legacy.hpp>

#ifdef _DEBUG           
#pragma comment(lib, "opencv_core249d.lib")   
#pragma comment(lib, "opencv_imgproc249d.lib")   //MAT processing   
#pragma comment(lib, "opencv_objdetect249d.lib") //HOGDescriptor   
//#pragma comment(lib, "opencv_gpu249d.lib")   
//#pragma comment(lib, "opencv_features2d249d.lib")   
#pragma comment(lib, "opencv_highgui249d.lib")   
#pragma comment(lib, "opencv_ml249d.lib")   
//#pragma comment(lib, "opencv_stitching249d.lib");   
//#pragma comment(lib, "opencv_nonfree249d.lib");   
#pragma comment(lib, "opencv_video249d.lib")
#pragma comment(lib, "opencv_legacy249d.lib")
#else   
#pragma comment(lib, "opencv_core249.lib")   
#pragma comment(lib, "opencv_imgproc249.lib")   
#pragma comment(lib, "opencv_objdetect249.lib")   
//#pragma comment(lib, "opencv_gpu249.lib")   
//#pragma comment(lib, "opencv_features2d249.lib")   
#pragma comment(lib, "opencv_highgui249.lib")   
#pragma comment(lib, "opencv_ml249.lib")   
//#pragma comment(lib, "opencv_stitching249.lib");   
//#pragma comment(lib, "opencv_nonfree249.lib");   
#pragma comment(lib, "opencv_video249d.lib")   
#endif   



using namespace cv;
using namespace std;

// (1) functions for calculating the likelihood
float calc_likelihood (IplImage * img, int x, int y)
{
 float b, g, r;
 float dist = 0.0, sigma = 50.0;

 b = img->imageData[img->widthStep * y + x * 3];       //B
 g = img->imageData[img->widthStep * y + x * 3 + 1];   //G
 r = img->imageData[img->widthStep * y + x * 3 + 2];   //R
 dist = sqrt (b * b + g * g + (255.0 - r) * (255.0 - r));

 return 1.0 / (sqrt (2.0 * CV_PI) * sigma) * expf (-dist * dist / (2.0 * sigma * sigma));
}

void ProccTimePrint( unsigned long Atime , string msg)   
{   
 unsigned long Btime=0;   
 float sec, fps;   
 Btime = getTickCount();   
 sec = (Btime - Atime)/getTickFrequency();   
 fps = 1/sec;   
 printf("%s %.4lf(sec) / %.4lf(fps) \n", msg.c_str(),  sec, fps );   
}   



int main ()
{

 float TakeTime;   
 unsigned long Atime, Btime;   

 int i, c;
 double w = 0.0, h = 0.0;
 CvCapture *capture = 0;
 IplImage *frame = 0;

 int n_stat = 4;
 int n_particle = 1000;
 vector<  float > vx(n_particle);
 vector<  float > vy(n_particle);

 CvConDensation *cond = 0;
 CvMat *lowerBound = 0;
 CvMat *upperBound = 0;

 int xx, yy;

 // (2)you want to create a capture structure with respect to the camera with the specified number by the command argument
 
 capture = cvCreateCameraCapture(0);

 // (3)The one frame captured, and obtains the capture size.
 frame = cvQueryFrame (capture);
 w = frame->width;
 h = frame->height;

 cvNamedWindow ("Condensation", CV_WINDOW_AUTOSIZE);

 // (4)Condensation create a structure.
 cond = cvCreateConDensation (n_stat, 0, n_particle);

 // (5) it will specify the minimum and maximum values of the state vector can be taken for each dimension.
 lowerBound = cvCreateMat (4, 1, CV_32FC1);
 upperBound = cvCreateMat (4, 1, CV_32FC1);

 cvmSet (lowerBound, 0, 0, 0.0);
 cvmSet (lowerBound, 1, 0, 0.0);
 cvmSet (lowerBound, 2, 0, -10); //-10.0);
 cvmSet (lowerBound, 3, 0, -10); //-10.0);
 cvmSet (upperBound, 0, 0, w);
 cvmSet (upperBound, 1, 0, h);
 cvmSet (upperBound, 2, 0, 10); //10.0);
 cvmSet (upperBound, 3, 0, 10); //10.0);

 // (6)Condensation Initialize the structure
 cvConDensInitSampleSet (cond, lowerBound, upperBound);

 // (7)ConDensation Specify the dynamics of the state vector in the algorithm
 cond->DynamMatr[0] = 1.0;
 cond->DynamMatr[1] = 0.0;
 cond->DynamMatr[2] = 1.0;
 cond->DynamMatr[3] = 0.0;
 cond->DynamMatr[4] = 0.0;
 cond->DynamMatr[5] = 1.0;
 cond->DynamMatr[6] = 0.0;
 cond->DynamMatr[7] = 1.0;
 cond->DynamMatr[8] = 0.0;
 cond->DynamMatr[9] = 0.0;
 cond->DynamMatr[10] = 1.0;
 cond->DynamMatr[11] = 0.0;
 cond->DynamMatr[12] = 0.0;
 cond->DynamMatr[13] = 0.0;
 cond->DynamMatr[14] = 0.0;
 cond->DynamMatr[15] = 1.0;

 // (8)re-set the noise parameters.

 while (1) {
  frame = cvQueryFrame (capture);
  Atime = getTickCount(); //시작 시간 
  float a=0,b=0,c=0,d=0,e=0;

  // (9) It calculates the likelihood for each particle.
  for (i = 0; i <  n_particle; i++) {
   xx = (int) (cond->flSamples[i][0]);
   yy = (int) (cond->flSamples[i][1]);

   vx[i] = cond->flSamples[i][0];
   vy[i] = cond->flSamples[i][1];


   if (xx <  0 || xx >= w || yy <  0 || yy >= h) {
    cond->flConfidence[i] = 0.0;
   }
   else {
    cond->flConfidence[i] = calc_likelihood (frame, xx, yy);
    cvCircle (frame, cvPoint (xx, yy), 1, CV_RGB (0, 0, 255), -1);   
    
   }
  }

  
  // (10)  estimate the state of the next model
  cvConDensUpdateByTime (cond);
  printf("crrection \n");
  
  ProccTimePrint( Atime , "time :"); //처리시간 출력  

  cv::Point statePt(cond->State[0], cond->State[1]);
  cvCircle (frame, statePt, 5, CV_RGB (255, 255, 255), 5);  
  printf("-----------\n");
  
  cvShowImage ("Condensation", frame);
  
  if (cvWaitKey (10) > 10 )
   break;

 }

 cvDestroyWindow ("Condensation");

 cvReleaseCapture (&capture);
 cvReleaseConDensation (&cond);
 cvReleaseMat (&lowerBound);
 cvReleaseMat (&upperBound);

 return 0;
}