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;
}




Comments

  1. I want to tell you :
    Thank you so match for you recommendation and for you replay.

    ReplyDelete

Post a Comment

Popular posts from this blog

OpenCV Stitching example (Stitcher class, Panorama)

(OpenCV Study) Background subtractor MOG, MOG2, GMG example source code (BackgroundSubtractorMOG, BackgroundSubtractorMOG2, BackgroundSubtractorGMG)

Example source code of extract HOG feature from images, save descriptor values to xml file, using opencv (using HOGDescriptor )

Real-time N camera stitching Class.

8 point algorithm (Matlab source code) / The method to get the Fundamental Matrix and the Essential matrix

Optical Flow sample source code using OpenCV

Video Stabilization example source code, (using cvFindHomography, cvWarpPerspective functions in openCV)

(OpenCV Study) calcOpticalFlowFarneback example source code ( dense optical flow )

yuv422(YUYV) to RGB and RGB to yuv422(YUYV), (Using OpenCV and TBB)

OpenCV Drawing Example, (line, circle, rectangle, ellipse, polyline, fillConvexPoly, putText, drawContours)